Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Shop mode #166

Closed
wants to merge 4 commits into from
Closed

Shop mode #166

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
744830b
draft of framework, will work on getting values in a bit
minhnguyenbhs Mar 11, 2024
9e5a993
the shop's source angle slope (and position i guess) seems to be the …
minhnguyenbhs Mar 13, 2024
07d2436
untuned
minhnguyenbhs Mar 14, 2024
a844659
added shop mode stop blue podium
minhnguyenbhs Mar 20, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
43 changes: 33 additions & 10 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,8 @@ public static final class FeatureFlags {
public static final boolean runDrive = true;

public static final boolean enableLEDS = false;

public static final boolean shopMode = true;
}

public static final class ConversionConstants {
Expand Down Expand Up @@ -95,11 +97,11 @@ public static final class DriveConstants {
}

public static final class FieldConstants {
public static final double lengthM = 16.451;
public static final double widthM = 8.211;
public static final double lengthM = !FeatureFlags.shopMode ? 16.451 : 16.451;
public static final double widthM = !FeatureFlags.shopMode ? 8.211 : 8.211;

public static final double midfieldLowThresholdM = 5.87;
public static final double midfieldHighThresholdM = 10.72;
public static final double midfieldLowThresholdM = !FeatureFlags.shopMode ? 5.87 : 5.87;
public static final double midfieldHighThresholdM = !FeatureFlags.shopMode ? 10.72 : 10.72;

public static final Rotation2d ampHeading = new Rotation2d(-Math.PI / 2);

Expand All @@ -114,12 +116,22 @@ public static final class FieldConstants {
public static final Rotation2d redRightHeading = Rotation2d.fromRadians(Math.PI / 2.0);

public static final Rotation2d redSourceHeading =
new Rotation2d(Math.PI * 4 / 3); // 60 degrees
!FeatureFlags.shopMode
? new Rotation2d(Math.PI * 4 / 3)
: // 60 degrees
new Rotation2d(Math.toRadians(180 - 114.3));
public static final Rotation2d blueSourceHeading =
new Rotation2d(Math.PI * 5 / 3); // 120 degrees
!FeatureFlags.shopMode
? new Rotation2d(Math.PI * 5 / 3)
: // 120 degrees
new Rotation2d(Math.toRadians(114.3));

public static final Translation2d fieldToRedSpeaker =
new Translation2d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42));
!FeatureFlags.shopMode
? new Translation2d(
Units.inchesToMeters(652.73), Units.inchesToMeters(218.42))
: new Translation2d(
Units.inchesToMeters(652.73), Units.inchesToMeters(218.42));

public static final Translation2d fieldToBlueSpeaker =
new Translation2d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42));
Expand All @@ -128,19 +140,25 @@ public static final class FieldConstants {
new Pose2d(1.39, 5.56, Rotation2d.fromDegrees(180));

public static final Pose2d robotAgainstRedSpeaker =
new Pose2d(15.19, 5.56, Rotation2d.fromDegrees(0));
!FeatureFlags.shopMode
? new Pose2d(15.19, 5.56, Rotation2d.fromDegrees(0))
: new Pose2d(15.19, 5.56, Rotation2d.fromDegrees(0));

public static final Pose2d robotAgainstBluePodium =
new Pose2d(2.57, 4.09, Rotation2d.fromDegrees(180));

public static final Pose2d robotAgainstRedPodium =
new Pose2d(13.93, 4.09, Rotation2d.fromDegrees(0));
!FeatureFlags.shopMode
? new Pose2d(13.93, 4.09, Rotation2d.fromDegrees(0))
: new Pose2d(13.93, 4.09, Rotation2d.fromDegrees(0));

public static final Pose2d robotAgainstBlueAmpZone =
new Pose2d(2.85, 7.68, Rotation2d.fromDegrees(-90));

public static final Pose2d robotAgainstRedAmpZone =
new Pose2d(13.74, 7.68, Rotation2d.fromDegrees(-90));
!FeatureFlags.shopMode
? new Pose2d(13.74, 7.68, Rotation2d.fromDegrees(-90))
: new Pose2d(13.74, 7.68, Rotation2d.fromDegrees(-90));
}

public static final class VisionConstants {
Expand Down Expand Up @@ -493,6 +511,11 @@ public static final class ScoringConstants {
public static final double intakeAngleToleranceRadians = 0.1;
// Math.PI / 2 - Units.degreesToRadians(40);

public static final double sourceIntakeAngleRad =
!FeatureFlags.shopMode
? 0.35
: 0.35; // FIELD ANGLE: 50 DEGREES, SHOP ANGLE: 38.9 DEGREES

public static final double shooterAmpVelocityRPM = 2000;

public static final double hoodHomeAmps = 40.0; // TODO: Find this
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -186,7 +186,8 @@ private void intake() {
}

private void sourceIntake() {
aimerIo.setAimAngleRad(0.35, true);
aimerIo.setAimAngleRad(ScoringConstants.sourceIntakeAngleRad, true);

shooterIo.setKickerVolts(-1);

shooterIo.setOverrideMode(true);
Expand Down
24 changes: 13 additions & 11 deletions src/main/java/frc/robot/utils/FieldFinder.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package frc.robot.utils;

import edu.wpi.first.math.MathUtil;
import frc.robot.Constants.FeatureFlags;
import java.awt.geom.Line2D;

public class FieldFinder {
Expand Down Expand Up @@ -97,22 +98,23 @@ public static boolean willIHitThis(
FieldLocations2024.STAGE_RED_RIGHT_SIDE_Y);
case BLUE_STAGE:
return willIHitTriangle(
x,
y,
dx,
dy,
FieldLocations2024.STAGE_BLUE_NEAR_SIDE_X,
FieldLocations2024.STAGE_BLUE_NEAR_SIDE_Y,
FieldLocations2024.STAGE_BLUE_LEFT_SIDE_X,
FieldLocations2024.STAGE_BLUE_LEFT_SIDE_Y,
FieldLocations2024.STAGE_BLUE_RIGHT_SIDE_X,
FieldLocations2024.STAGE_BLUE_RIGHT_SIDE_Y);
x,
y,
dx,
dy,
FieldLocations2024.STAGE_BLUE_NEAR_SIDE_X,
FieldLocations2024.STAGE_BLUE_NEAR_SIDE_Y,
FieldLocations2024.STAGE_BLUE_LEFT_SIDE_X,
FieldLocations2024.STAGE_BLUE_LEFT_SIDE_Y,
FieldLocations2024.STAGE_BLUE_RIGHT_SIDE_X,
FieldLocations2024.STAGE_BLUE_RIGHT_SIDE_Y)
&& !FeatureFlags.shopMode;
case RED_WING:
return x > FieldLocations2024.WING_RED_END_X
|| x + dx > FieldLocations2024.WING_RED_END_X;
case BLUE_WING:
return x < FieldLocations2024.WING_BLUE_END_X
|| x + dx < FieldLocations2024.WING_BLUE_END_X;
|| x + dx < FieldLocations2024.WING_BLUE_END_X && !FeatureFlags.shopMode;
case MIDDLE:
return (x < FieldLocations2024.WING_RED_END_X
|| x + dx < FieldLocations2024.WING_RED_END_X)
Expand Down