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

Jump to bottom

Feat/automate low scoring #37

Open
wants to merge 3 commits into
base: fix/testing-tuning-4-12
Choose a base branch
from
Open

Feat/automate low scoring #37

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
b99472c
Add automating low scoring
viveknadig282 Apr 14, 2023
4958e5c
Delete New Path.path
adityapawar1 Apr 14, 2023
20c9ef0
Fixing requested changes
viveknadig282 Apr 14, 2023
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
1 change: 1 addition & 0 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 @@ -41,6 +41,7 @@ public static final class FeatureFlags {
public static final boolean kUsePrefs = true;
public static final boolean kAutoScoreEnabled = true;
public static final boolean kAutoOuttakeEnabled = true;
public static final boolean kDirectionBasedLowScoreEnabled = true;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you make this false before we test

public static final boolean kOperatorManualArmControlEnabled = true;
public static final boolean kSwerveAccelerationLimitingEnabled = true;
public static final boolean kLocalizationStdDistanceBased = true;
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/PitTestRoutine.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@ private Command elevatorCommands() {
new SetElevatorExtension(elevatorSubsystem, Elevator.ElevatorPreset.ANY_PIECE_MID)
.until(driver.b());
Command setElevatorHeightLOW =
new SetElevatorExtension(elevatorSubsystem, Elevator.ElevatorPreset.ANY_PIECE_LOW)
new SetElevatorExtension(elevatorSubsystem, Elevator.ElevatorPreset.ANY_PIECE_LOW_BACK)
.until(driver.b());
Command setElevatorToStart =
new SetElevatorExtension(elevatorSubsystem, kElevatorStartingPosition).until(driver.b());
Expand Down
8 changes: 6 additions & 2 deletions src/main/java/frc/robot/arm/Arm.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,8 @@ public class Arm extends SubsystemBase implements CANTestable, Loggable {
public enum ArmPreset {
STOW_CUBE(kStowRotationCube),
STOW_CONE(kStowRotationCone),
ANY_PIECE_LOW(kAnyPieceLowRotation),
ANY_PIECE_LOW_BACK(kAnyPieceLowBackRotation),
ANY_PIECE_LOW_FRONT(kAnyPieceLowFrontRotation),
CUBE_MID(kCubeMidRotation),
CONE_MID(kConeMidRotation),
CUBE_HIGH(kCubeHighRotation),
Expand Down Expand Up @@ -204,7 +205,10 @@ public static void loadArmPreferences() {
Preferences.initDouble(
kArmPositionKeys.get(ArmPreset.STOW_CUBE), kStowRotationCube.getRadians());
Preferences.initDouble(
kArmPositionKeys.get(ArmPreset.ANY_PIECE_LOW), kAnyPieceLowRotation.getRadians());
kArmPositionKeys.get(ArmPreset.ANY_PIECE_LOW_BACK), kAnyPieceLowBackRotation.getRadians());
Preferences.initDouble(
kArmPositionKeys.get(ArmPreset.ANY_PIECE_LOW_FRONT),
kAnyPieceLowFrontRotation.getRadians());
Preferences.initDouble(kArmPositionKeys.get(ArmPreset.CUBE_MID), kCubeMidRotation.getRadians());
Preferences.initDouble(kArmPositionKeys.get(ArmPreset.CONE_MID), kConeMidRotation.getRadians());
Preferences.initDouble(
Expand Down
10 changes: 7 additions & 3 deletions src/main/java/frc/robot/arm/ArmConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,9 @@ public final class ArmConstants {
public static final Rotation2d kConeMidRotation = new Rotation2d(0);
public static final Rotation2d kCubeHighRotation = new Rotation2d(0);
public static final Rotation2d kConeHighRotation = new Rotation2d(0);
public static final Rotation2d kAnyPieceLowRotation = Rotation2d.fromDegrees(150);
public static final Rotation2d kAnyPieceLowFrontRotation = Rotation2d.fromDegrees(0);

public static final Rotation2d kAnyPieceLowBackRotation = Rotation2d.fromDegrees(150);

public static final Rotation2d kStandingConeGroundIntakeRotation = Rotation2d.fromDegrees(150);
public static final Rotation2d kTippedConeGroundIntakeRotation = Rotation2d.fromDegrees(180);
Expand All @@ -71,7 +73,8 @@ public static class ArmPreferencesKeys {
Map.ofEntries(
Map.entry(Arm.ArmPreset.STOW_CONE, "kStowRotationCone"),
Map.entry(Arm.ArmPreset.STOW_CUBE, "kStowRotationCube"),
Map.entry(Arm.ArmPreset.ANY_PIECE_LOW, "kAnyPieceLowRotation"),
Map.entry(Arm.ArmPreset.ANY_PIECE_LOW_BACK, "kAnyPieceLowBackRotation"),
Map.entry(Arm.ArmPreset.ANY_PIECE_LOW_FRONT, "kAnyPieceLowFrontRotation"),
Map.entry(Arm.ArmPreset.CUBE_MID, "kCubeMidRotation"),
Map.entry(Arm.ArmPreset.CONE_MID, "kConeMidRotation"),
Map.entry(Arm.ArmPreset.CUBE_HIGH, "kCubeHighRotation"),
Expand All @@ -87,7 +90,8 @@ public static class ArmPreferencesKeys {
Map.ofEntries(
Map.entry(Arm.ArmPreset.STOW_CONE, kStowRotationCone),
Map.entry(Arm.ArmPreset.STOW_CUBE, kStowRotationCube),
Map.entry(Arm.ArmPreset.ANY_PIECE_LOW, kAnyPieceLowRotation),
Map.entry(Arm.ArmPreset.ANY_PIECE_LOW_BACK, kAnyPieceLowBackRotation),
Map.entry(Arm.ArmPreset.ANY_PIECE_LOW_FRONT, kAnyPieceLowFrontRotation),
Map.entry(Arm.ArmPreset.CUBE_MID, kCubeMidRotation),
Map.entry(Arm.ArmPreset.CONE_MID, kConeMidRotation),
Map.entry(Arm.ArmPreset.CUBE_HIGH, kCubeHighRotation),
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/auto/AutoPaths.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,7 @@ public void sendCommandsToChooser() {
new SetEndEffectorState(
elevatorSubsystem,
armSubsystem,
SetEndEffectorState.EndEffectorPreset.SCORE_ANY_LOW)
SetEndEffectorState.EndEffectorPreset.SCORE_ANY_LOW_FRONT)
.asProxy()
.withName("coneLow"));

Expand Down Expand Up @@ -136,7 +136,7 @@ public void sendCommandsToChooser() {
new SetEndEffectorState(
elevatorSubsystem,
armSubsystem,
SetEndEffectorState.EndEffectorPreset.SCORE_ANY_LOW)
SetEndEffectorState.EndEffectorPreset.SCORE_ANY_LOW_FRONT)
.asProxy()
.withName("cubeLow"));

Expand Down
49 changes: 38 additions & 11 deletions src/main/java/frc/robot/auto/pathgeneration/commands/AutoScore.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,9 +8,11 @@
package frc.robot.auto.pathgeneration.commands;

import static frc.robot.Constants.FeatureFlags.kAutoOuttakeEnabled;
import static frc.robot.Constants.FeatureFlags.kDirectionBasedLowScoreEnabled;
import static frc.robot.auto.dynamicpathgeneration.DynamicPathConstants.*;
import static frc.robot.led.LEDConstants.*;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
Expand Down Expand Up @@ -54,6 +56,7 @@ public enum GridScoreHeight {
private BooleanSupplier cancelCommand;
private BooleanSupplier isCurrentLEDPieceCone;
private BooleanSupplier isAutoScoreMode;
private boolean isRedAlliance;

public AutoScore(
SwerveDrive swerveDrive,
Expand All @@ -77,8 +80,25 @@ public AutoScore(
this.cancelCommand = cancelCommand;
}

// This checks if the robot's front side is facing the target or if the robot's backside is facing
// the target

private boolean isAlignedFront() {
if(!kDirectionBasedLowScoreEnabled) return false;

double currentRobotAngle =
MathUtil.angleModulus(swerveSubsystem.getPose().getRotation().getRadians());
viveknadig282 marked this conversation as resolved.
Show resolved Hide resolved
if (isRedAlliance) {
return Math.abs(currentRobotAngle) < Math.PI/2;
} else {
viveknadig282 marked this conversation as resolved.
Show resolved Hide resolved
return Math.abs(currentRobotAngle) > Math.PI/2;
}
}

@Override
public void initialize() {
this.isRedAlliance = DriverStation.getAlliance() == Alliance.Red;

System.out.println(
"Is running auto score instead of presets: " + isAutoScoreMode.getAsBoolean());
if (!isAutoScoreMode.getAsBoolean()) {
Expand Down Expand Up @@ -113,10 +133,16 @@ public void initialize() {
.schedule();
break;
case LOW:
new SetEndEffectorState(
elevatorSubsystem,
armSubsystem,
SetEndEffectorState.EndEffectorPreset.SCORE_ANY_LOW)
new ConditionalCommand(
new SetEndEffectorState(
elevatorSubsystem,
armSubsystem,
SetEndEffectorState.EndEffectorPreset.SCORE_ANY_LOW_FRONT),
new SetEndEffectorState(
elevatorSubsystem,
armSubsystem,
SetEndEffectorState.EndEffectorPreset.SCORE_ANY_LOW_BACK),
this::isAlignedFront)
.schedule();
break;
}
Expand All @@ -128,7 +154,7 @@ public void initialize() {

// Get scoring location id from SD
int locationId = (int) SmartDashboard.getNumber("guiColumn", -1);
if (DriverStation.getAlliance() == Alliance.Blue) {
if (!isRedAlliance) {
locationId = 8 - locationId;
}
if (0 > locationId || locationId > 8) {
Expand All @@ -142,7 +168,7 @@ public void initialize() {
GamePiece scoringGamePiece = kScoringLocationPiece[locationId];

System.out.println("Running: Go to grid (id: " + locationId + ") from " + start);
if (DriverStation.getAlliance() == Alliance.Red) {
if (isRedAlliance) {
scoringWaypoint = PathUtil.flip(scoringWaypoint);
}
Command moveToScoringWaypoint;
Expand Down Expand Up @@ -198,10 +224,11 @@ public void initialize() {
default:
scoringLocation = kBottomBlueScoringPoses[locationId];
moveArmElevatorToPreset =
new SetEndEffectorState(
elevatorSubsystem,
armSubsystem,
SetEndEffectorState.EndEffectorPreset.SCORE_ANY_LOW);
new SetEndEffectorState(
elevatorSubsystem,
armSubsystem,
SetEndEffectorState.EndEffectorPreset.SCORE_ANY_LOW_BACK);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

When we auto score we score in the front so we should change this to SCORE_ANY_LOW_FRONT

break;
}

if (FeatureFlags.kIntakeAutoScoreDistanceSensorOffset) {
Expand All @@ -211,7 +238,7 @@ public void initialize() {
new Translation2d(0, intakeSubsystem.getGamePieceOffset()), new Rotation2d()));
}

if (DriverStation.getAlliance() == Alliance.Red) {
if (isRedAlliance) {
scoringLocation = PathUtil.flip(scoringLocation);
viveknadig282 marked this conversation as resolved.
Show resolved Hide resolved
}

Expand Down
8 changes: 6 additions & 2 deletions src/main/java/frc/robot/elevator/Elevator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,8 @@ public enum ElevatorPreset {
CUBE_HIGH(kCubeHighPosition),
CONE_HIGH(kConeHighPosition),
ANY_PIECE_MID(kAnyPieceMidPosition),
ANY_PIECE_LOW(kAnyPieceLowPosition),
ANY_PIECE_LOW_BACK(kAnyPieceLowBackPosition),
ANY_PIECE_LOW_FRONT(kAnyPieceLowFrontPosition),
GROUND_INTAKE(kGroundIntakePosition),
DOUBLE_SUBSTATION_CONE(kConeDoubleSubstationPosition),
DOUBLE_SUBSTATION_CUBE(kCubeDoubleSubstationPosition);
Expand Down Expand Up @@ -179,7 +180,10 @@ public static void loadElevatorPreferences() {
Preferences.initDouble(
kElevatorPositionKeys.get(Elevator.ElevatorPreset.CONE_HIGH), kConeHighPosition);
Preferences.initDouble(
kElevatorPositionKeys.get(Elevator.ElevatorPreset.ANY_PIECE_LOW), kAnyPieceLowPosition);
kElevatorPositionKeys.get(Elevator.ElevatorPreset.ANY_PIECE_LOW_BACK),
kAnyPieceLowBackPosition);
Preferences.initDouble(
kElevatorPositionKeys.get(ElevatorPreset.ANY_PIECE_LOW_FRONT), kAnyPieceLowFrontPosition);
Preferences.initDouble(
kElevatorPositionKeys.get(Elevator.ElevatorPreset.ANY_PIECE_MID), kAnyPieceMidPosition);
Preferences.initDouble(
Expand Down
10 changes: 7 additions & 3 deletions src/main/java/frc/robot/elevator/ElevatorConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,9 @@ public final class ElevatorConstants {
public static final double kCubeHighPosition = Units.inchesToMeters(57);
public static final double kConeHighPosition = Units.inchesToMeters(57);
public static final double kAnyPieceMidPosition = Units.inchesToMeters(36);
public static final double kAnyPieceLowPosition = Units.inchesToMeters(6);
public static final double kAnyPieceLowFrontPosition = Units.inchesToMeters(6);

public static final double kAnyPieceLowBackPosition = Units.inchesToMeters(6);
public static final double kGroundIntakePosition = Units.inchesToMeters(0);

// https://www.reca.lc/linear
Expand Down Expand Up @@ -65,7 +67,8 @@ public static class ElevatorPreferencesKeys {
Elevator.ElevatorPreset.STOW_CUBE, "kStowPositionCube",
Elevator.ElevatorPreset.CUBE_HIGH, "kCubeHighPositionMeters",
Elevator.ElevatorPreset.CONE_HIGH, "kConeHighPositionMeters",
Elevator.ElevatorPreset.ANY_PIECE_LOW, "kAnyPieceLowPositionMeters",
Elevator.ElevatorPreset.ANY_PIECE_LOW_BACK, "kAnyPieceLowBackPositionMeters",
Elevator.ElevatorPreset.ANY_PIECE_LOW_FRONT, "kAnyPieceLowFrontPositionMeters",
Elevator.ElevatorPreset.ANY_PIECE_MID, "kAnyPieceMidPositionMeters",
Elevator.ElevatorPreset.GROUND_INTAKE, "kGroundIntakePositionMeters",
Elevator.ElevatorPreset.DOUBLE_SUBSTATION_CONE, "kDoubleSubstationPositionConeMeters",
Expand All @@ -77,7 +80,8 @@ public static class ElevatorPreferencesKeys {
Elevator.ElevatorPreset.STOW_CUBE, kCubeStowPosition,
Elevator.ElevatorPreset.CUBE_HIGH, kCubeHighPosition,
Elevator.ElevatorPreset.CONE_HIGH, kConeHighPosition,
Elevator.ElevatorPreset.ANY_PIECE_LOW, kAnyPieceLowPosition,
Elevator.ElevatorPreset.ANY_PIECE_LOW_BACK, kAnyPieceLowBackPosition,
Elevator.ElevatorPreset.ANY_PIECE_LOW_FRONT, kAnyPieceLowFrontPosition,
Elevator.ElevatorPreset.ANY_PIECE_MID, kAnyPieceMidPosition,
Elevator.ElevatorPreset.GROUND_INTAKE, kGroundIntakePosition,
Elevator.ElevatorPreset.DOUBLE_SUBSTATION_CONE, kConeDoubleSubstationPosition,
Expand Down
5 changes: 4 additions & 1 deletion src/main/java/frc/robot/elevator/commands/SetEndEffectorState.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,10 @@ public class SetEndEffectorState extends ParallelCommandGroup {
public enum EndEffectorPreset {
SCORE_CONE_HIGH(Arm.ArmPreset.CONE_HIGH, Elevator.ElevatorPreset.CONE_HIGH),
SCORE_CONE_MID(Arm.ArmPreset.CONE_MID, Elevator.ElevatorPreset.ANY_PIECE_MID),
SCORE_ANY_LOW(Arm.ArmPreset.ANY_PIECE_LOW, Elevator.ElevatorPreset.ANY_PIECE_LOW),
SCORE_ANY_LOW_BACK(
Arm.ArmPreset.ANY_PIECE_LOW_BACK, Elevator.ElevatorPreset.ANY_PIECE_LOW_BACK),
SCORE_ANY_LOW_FRONT(
Arm.ArmPreset.ANY_PIECE_LOW_FRONT, Elevator.ElevatorPreset.ANY_PIECE_LOW_FRONT),
DOUBLE_SUBSTATION_CONE(
Arm.ArmPreset.DOUBLE_SUBSTATION_CONE, Elevator.ElevatorPreset.DOUBLE_SUBSTATION_CONE),
STOW_CONE(Arm.ArmPreset.STOW_CONE, Elevator.ElevatorPreset.STOW_CONE),
Expand Down