Skip to content

Commit

Permalink
add pre scoring 2 for the high nodes
Browse files Browse the repository at this point in the history
  • Loading branch information
@varun7654
varun7654 committed Apr 11, 2023
1 parent 9a638ce commit 67fea42
Show file tree
Hide file tree
Showing 3 changed files with 51 additions and 24 deletions.
16 changes: 8 additions & 8 deletions build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,13 +29,13 @@ deploy {

frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
gcType = 'other'
jvmArgs.add("-Don.robot=true") // This won't be used in simulation
jvmArgs.add("-Dcom.sun.management.jmxremote=true")
jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198")
jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false")
jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false")
jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false")
jvmArgs.add("-Djava.rmi.server.hostname=10.34.76.2")
// jvmArgs.add("-Don.robot=true") // This won't be used in simulation
// jvmArgs.add("-Dcom.sun.management.jmxremote=true")
// jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198")
// jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false")
// jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false")
// jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false")
// jvmArgs.add("-Djava.rmi.server.hostname=10.34.76.2")

jvmArgs.add("-XX:+UnlockExperimentalVMOptions")
jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M")
Expand Down Expand Up @@ -102,7 +102,7 @@ dependencies {
implementation 'org.apache.commons:commons-math3:3.6.1'
implementation 'com.google.guava:guava:31.1-jre'

implementation "com.dacubeking:autobuilder-robot:2.2.6"
implementation "com.dacubeking:autobuilder-robot:2.2.7"
implementation 'org.joml:joml:1.10.4'


Expand Down
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,8 @@ public final class Constants {
1000000000; // 1 GB

public static final double GYRO_IS_FLAT_FOR_PICKUP_THRESHOLD_DEGREES = 5;
public static final double PRE_SCORE_TIME_S = 1;
public static final double PRE_SCORE_TIME_S = 1.3;
public static final double PRE_SCORE_TIME_2_S = 0.4;
public static final double SCORE_TIME_S = 0.0;
public static final double SCORE_POSITION_ERROR_SQUARED = 0.08 * 0.8; // 4 cm
// Drive Constants
Expand Down Expand Up @@ -107,7 +108,6 @@ public final class Constants {
new SwerveModuleState(0, Rotation2d.fromDegrees(45))
};

// 0.307975 is 12.125 in inches
public static final @NotNull Translation2d SWERVE_LEFT_FRONT_LOCATION = new Translation2d(0.26352, 0.26352);
public static final @NotNull Translation2d SWERVE_LEFT_BACK_LOCATION = new Translation2d(-0.26352, 0.26352);
public static final @NotNull Translation2d SWERVE_RIGHT_FRONT_LOCATION = new Translation2d(0.26352, -0.26352);
Expand Down Expand Up @@ -233,7 +233,7 @@ public final class Constants {

public static final double GRABBER_ROLLER_VOLTAGE = -6;
public static final double GRABBER_ROLLER_IDLE = -0;
public static final double GRABBER_CLOSE_THRESHOLD_DEGREES = 40;
public static final double GRABBER_CLOSE_THRESHOLD_DEGREES = 48;
public static final double GRABBER_OPEN_THRESHOLD_DEGREES = 55;
public static final boolean USE_GRABBER_ENCODER = !IS_PRACTICE;
public static final boolean GRABBER_WHEELS_USED = false;
Expand Down Expand Up @@ -317,7 +317,7 @@ public enum KinematicLimits {
public static final double UPPER_PICKUP_POSITION_Y = -2.05892 + Units.inchesToMeters(2);
public static final double PICKUP_POSITION_X_OFFSET_FROM_WALL = FIELD_WIDTH_METERS - 15.2 + Units.inchesToMeters(-1.5);

public static final double SCORING_POSITION_OFFSET_CONE_FROM_WALL = 0.05;
public static final double SCORING_POSITION_OFFSET_CONE_FROM_WALL = 0.05 + Units.inchesToMeters(1);
public static final double SCORING_POSITION_OFFSET_CUBE_FROM_WALL = 0.05 + Units.inchesToMeters(3.5);

public static final double SINGLE_STATION_RED_X = 14.39;
Expand Down
51 changes: 39 additions & 12 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -423,6 +423,8 @@ enum WantedMechanismState {
STOWED(false),
SCORING(false),
PRE_SCORING(false),
PRE_SCORING_2(false),

FLOOR_PICKUP(true),
TIPPED_FLOOR_PICKUP(true),
STATION_PICKUP_DOUBLE(true),
Expand Down Expand Up @@ -464,16 +466,25 @@ enum WantedMechanismState {
MechanismStates.CONE_HIGH_SCORING,
MechanismStates.CUBE_HIGH_SCORING,
MechanismStates.FINAL_CONE_MIDDLE_SCORING,
MechanismStates.FINAL_CONE_HIGH_SCORING
MechanismStates.FINAL_CONE_HIGH_SCORING,
MechanismStates.PRE_SCORING_CONE_HIGH_2,
MechanismStates.PRE_SCORING
);

private final List<MechanismStates> autoReleaseWhenAtPosition = List.of(
MechanismStates.FINAL_CONE_MIDDLE_SCORING,
MechanismStates.FINAL_CONE_HIGH_SCORING
);

private final List<WantedMechanismState> wantedStatesConsideredStowed = List.of(
WantedMechanismState.STOWED,
WantedMechanismState.PRE_SCORING,
WantedMechanismState.PRE_SCORING_2
);


private boolean hasGoneToPreScore = false;
private boolean hasGoneToPreScore2 = false;
private boolean hasGoneToScore = false;


Expand Down Expand Up @@ -509,6 +520,7 @@ public void teleopPeriodic() {
hasReachedAutoAlignPosition = false;
assert teleopDrivingAutoAlignPosition != null;
hasGoneToPreScore = false;
hasGoneToPreScore2 = false;
hasGoneToScore = false;
} else if (teleopDrivingAutoAlignPosition == null) {
// We're not recalculating the grid position b/c we only need to recalculate the path because the operator
Expand Down Expand Up @@ -555,6 +567,13 @@ && dist2(autoDriveAlignError) <= SCORE_POSITION_ERROR_SQUARED
// We're done driving let's score
wantedMechanismState = WantedMechanismState.SCORING;
hasGoneToScore = true;
hasGoneToPreScore2 = true;
hasGoneToPreScore = true;
} else if (drive.getRemainingAutoDriveTime() <= PRE_SCORE_TIME_2_S
&& scoringPositionManager.getSelectedPosition().getLevel() == 2
&& !hasGoneToPreScore2) {
wantedMechanismState = WantedMechanismState.PRE_SCORING_2;
hasGoneToPreScore2 = true;
hasGoneToPreScore = true;
} else if (drive.getRemainingAutoDriveTime() <= PRE_SCORE_TIME_S
&& !hasGoneToPreScore) {
Expand Down Expand Up @@ -599,6 +618,15 @@ && dist2(autoDriveAlignError) <= SCORE_POSITION_ERROR_SQUARED
wantedMechanismState = WantedMechanismState.STOWED;
}

if (wantedMechanismState == WantedMechanismState.PRE_SCORING_2
&& scoringPositionManager.getSelectedPosition().getLevel() != 2) {
if (scoringPositionManager.getSelectedPosition().getLevel() != 1) {
wantedMechanismState = WantedMechanismState.PRE_SCORING;
} else {
wantedMechanismState = WantedMechanismState.STOWED;
}
}

if (xbox.getRisingEdge(XBOX_RESET_HEADING)) {
if (isRed()) {
robotTracker.resetPose(new Pose2d(robotTracker.getLatestPose().getTranslation(), Rotation2d.fromDegrees(0)));
Expand Down Expand Up @@ -629,9 +657,10 @@ && dist2(autoDriveAlignError) <= SCORE_POSITION_ERROR_SQUARED
}

if (stick.getRisingEdge(STICK_TOGGLE_SCORING)) {
if (wantedMechanismState == WantedMechanismState.STOWED || wantedMechanismState == WantedMechanismState.PRE_SCORING) {
if (wantedStatesConsideredStowed.contains(wantedMechanismState)) {
wantedMechanismState = WantedMechanismState.SCORING;
hasGoneToPreScore = true;
hasGoneToPreScore2 = true;
hasGoneToScore = true;
} else {
setStowed();
Expand All @@ -640,7 +669,7 @@ && dist2(autoDriveAlignError) <= SCORE_POSITION_ERROR_SQUARED

if (stick.getRisingEdge(STICK_TOGGLE_FLOOR_PICKUP) ||
xbox.getRisingEdge(CONTROLLER_TOGGLE_FLOOR_PICKUP, 0.1)) {
if (wantedMechanismState == WantedMechanismState.STOWED) {
if (wantedStatesConsideredStowed.contains(wantedMechanismState)) {
wantedMechanismState = WantedMechanismState.FLOOR_PICKUP;
isGrabberOpen = true;
} else {
Expand All @@ -649,7 +678,7 @@ && dist2(autoDriveAlignError) <= SCORE_POSITION_ERROR_SQUARED
}

if (xbox.getRisingEdge(CONTROLLER_TOGGLE_TIPPED_FLOOR_PICKUP, 0.1) && false) {
if (wantedMechanismState == WantedMechanismState.STOWED) {
if (wantedStatesConsideredStowed.contains(wantedMechanismState)) {
wantedMechanismState = WantedMechanismState.TIPPED_FLOOR_PICKUP;
isGrabberOpen = true;
} else {
Expand All @@ -658,7 +687,7 @@ && dist2(autoDriveAlignError) <= SCORE_POSITION_ERROR_SQUARED
}

if (stick.getRisingEdge(STICK_TOGGLE_PICKUP_DOUBLE)) {
if (wantedMechanismState == WantedMechanismState.STOWED) {
if (wantedStatesConsideredStowed.contains(wantedMechanismState)) {
wantedMechanismState = WantedMechanismState.STATION_PICKUP_DOUBLE;
isGrabberOpen = true;
} else {
Expand All @@ -667,7 +696,7 @@ && dist2(autoDriveAlignError) <= SCORE_POSITION_ERROR_SQUARED
}

if (stick.getRisingEdge(STICK_TOGGLE_PICKUP_SINGLE)) {
if (wantedMechanismState == WantedMechanismState.STOWED) {
if (wantedStatesConsideredStowed.contains(wantedMechanismState)) {
wantedMechanismState = WantedMechanismState.STATION_PICKUP_SINGLE;
isGrabberOpen = true;
} else {
Expand All @@ -684,10 +713,11 @@ && dist2(autoDriveAlignError) <= SCORE_POSITION_ERROR_SQUARED
}

if (xbox.getRisingEdge(XBOX_TOGGLE_MECH)) {
if (wantedMechanismState == WantedMechanismState.STOWED || wantedMechanismState == WantedMechanismState.PRE_SCORING) {
if (wantedStatesConsideredStowed.contains(wantedMechanismState)) {
if (isOnAllianceSide()) {
wantedMechanismState = WantedMechanismState.SCORING;
hasGoneToPreScore = true;
hasGoneToPreScore2 = true;
hasGoneToScore = true;
} else {
if ((isRed() && robotTracker.getLatestPose().getRotation()
Expand Down Expand Up @@ -736,6 +766,7 @@ && dist2(autoDriveAlignError) <= SCORE_POSITION_ERROR_SQUARED
}
}
case PRE_SCORING -> mechanismStateManager.setState(MechanismStates.PRE_SCORING);
case PRE_SCORING_2 -> mechanismStateManager.setState(MechanismStates.PRE_SCORING_CONE_HIGH_2);
case FLOOR_PICKUP -> mechanismStateManager.setState(MechanismStates.FLOOR_PICKUP);
case TIPPED_FLOOR_PICKUP -> mechanismStateManager.setState(MechanismStates.TIPPED_FLOOR_PICKUP);
case STATION_PICKUP_DOUBLE -> mechanismStateManager.setState(MechanismStates.DOUBLE_STATION_PICKUP);
Expand Down Expand Up @@ -860,16 +891,12 @@ && dist2(autoDriveAlignError) <= SCORE_POSITION_ERROR_SQUARED
if (grabber.isAutoGrabEnabled()) {

// Note: due to execution order, this is the gyro angle from the previous loop (~20ms ago)
double gyroAngle = robotTracker.getRawGyroAngle().getY();

// We don't care if we tilt forward b/c that means we're picking up from the base. We only care if we're tilted
// back (making it so that the grabber would pick up from the tip of the cone) which is why the tilt check is only
// in one direction.
boolean isFlat = Math.abs(Math.toDegrees(gyroAngle)) < GYRO_IS_FLAT_FOR_PICKUP_THRESHOLD_DEGREES;

if ((grabber.isOpen() || !isGrabberOpen) && mechanismStateManager.isMechAtFinalPos() &&
// Only care about the flatness if we're in doing a double substation pickup
(wantedMechanismState != WantedMechanismState.STATION_PICKUP_DOUBLE || isFlat)) {
if ((grabber.isOpen() || !isGrabberOpen) && mechanismStateManager.isMechAtFinalPos()) {
closeGrabber();
isGrabberOpen = false;
// We're enabling auto grab mode. The limit switch will prevent the grabber from closing until it detects a game piece
Expand Down

0 comments on commit 67fea42

Please sign in to comment.