Skip to content

Commit

Permalink
Merge pull request #62 from grt192/shuttle
Browse files Browse the repository at this point in the history 
allows for robot to shuttle notes
  • Loading branch information
@penguin212
penguin212 authored Apr 2, 2024
2 parents 59c6971 + 2df9312 commit 3b43676
Show file tree
Hide file tree
Showing 8 changed files with 147 additions and 162 deletions.
2 changes: 1 addition & 1 deletion src/main/deploy/pathplanner/navgrid.json
View file

Large diffs are not rendered by default.

16 changes: 3 additions & 13 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,9 +83,9 @@ public static class SwerveConstants {
public static final Translation2d BL_POS = new Translation2d(-MODULE_DIST, MODULE_DIST);
public static final Translation2d BR_POS = new Translation2d(-MODULE_DIST, -MODULE_DIST);

public static final Translation2d BLUE_SPEAKER_POS = new Translation2d(Units.inchesToMeters(-1.50 + 17.5),
public static final Translation2d BLUE_SPEAKER_POS = new Translation2d(Units.inchesToMeters(-1.50 + 8),
Units.inchesToMeters(218.42));
public static final Translation2d RED_SPEAKER_POS = new Translation2d(Units.inchesToMeters(652.73 - 17.5),
public static final Translation2d RED_SPEAKER_POS = new Translation2d(Units.inchesToMeters(652.73 - 8),
Units.inchesToMeters(218.42));
public static final double SPEAKER_TO_SPEAKER = Units.inchesToMeters(651.23);
}
Expand Down Expand Up @@ -130,17 +130,7 @@ public static class ShooterConstants {
public static final double MAX_FLYWHEEL_RPS = 6380.0 / 60;
public static final double MIN_SHOOTER_DISTANCE = 1.08;
public static final double MAX_SHOOTER_DISTANCE = 8;
public static final double FLYWHEEL_SHUTTLE_SPEED = 0.3;

//center of red speaker: (652.73 218.42)
public static final double RED_X = Units.inchesToMeters(652.73 + 9.05);
public static final double RED_Y = Units.inchesToMeters(218.42);

//center of blue speaker: (-1.50 218.42)
public static final double BLUE_X = Units.inchesToMeters(-1.5 + 9.05);
public static final double BLUE_Y = Units.inchesToMeters(218.42);


public static final double FLYWHEEL_SHUTTLE_SPEED = 0.3;
}

/** Constants for Climb Subsystem. */
Expand Down
96 changes: 54 additions & 42 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,13 +28,16 @@
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.POVButton;
import frc.robot.Constants.SwerveConstants;
import frc.robot.commands.auton.AutonBuilder;
import frc.robot.commands.climb.ClimbLowerCommand;
import frc.robot.commands.climb.ClimbRaiseCommand;
Expand Down Expand Up @@ -109,13 +112,16 @@ public class RobotContainer {
XboxController.Button.kLeftStick.value);
private final JoystickButton rightStickButton = new JoystickButton(mechController,
XboxController.Button.kRightStick.value);
private final POVButton dPadRight = new POVButton(mechController, 90);

private final GenericHID switchboard = new GenericHID(3);
private final JoystickButton offsetUpButton = new JoystickButton(switchboard, 7);
private final JoystickButton offsetDownButton = new JoystickButton(switchboard, 8);

private final JoystickButton shuttleNotes = new JoystickButton(switchboard, 6);
private final JoystickButton elevatorToZero = new JoystickButton(switchboard, 1);
private final JoystickButton shuttleNotesDefaultSpeed = new JoystickButton(switchboard, 6);

private UsbCamera driverCamera;
private MjpegServer driverCameraServer;

Expand Down Expand Up @@ -237,16 +243,10 @@ private void configureBindings() {
driveController.getRotatePower()
);
} else {
if (driveController.getSwerveAimMode()) {
swerveSubsystem.setSwerveAimDrivePowers(
driveController.getForwardPower(),
driveController.getLeftPower());
} else {
swerveSubsystem.setDrivePowers(
driveController.getForwardPower(),
driveController.getLeftPower(),
driveController.getRotatePower());
}
swerveSubsystem.setDrivePowers(
driveController.getForwardPower(),
driveController.getLeftPower(),
driveController.getRotatePower());
}

xError.setValue(xPID.getPositionError());
Expand All @@ -259,6 +259,46 @@ private void configureBindings() {
swerveSubsystem.resetDriverHeading();
}));

/* SWERVE BINDINGS */

/* Shooter Aim -- Holding down the button will change the shooter's pitch to aim it at the speaker. */
// drive

/* Amp Align -- Pressing and holding the button will cause the robot to automatically path find to the amp.
* Releasing the button will stop the robot (and the path finding). */
driveController.getAmpAlign().onTrue(new InstantCommand(
() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.AUTO_ALIGN, 1)
).andThen(new ParallelRaceGroup(
AlignCommand.getAmpAlignCommand(swerveSubsystem, fmsSubsystem.isRedAlliance()),
new ConditionalWaitCommand(
() -> !driveController.getAmpAlign().getAsBoolean()))
).andThen(new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1)))
);

/* Note align -- deprecated, new version in the works*/
driveController.getNoteAlign().onTrue(
new NoteAlignCommand(swerveSubsystem, noteDetector, driveController)
.unless(() -> noteDetector.getNote().isEmpty())
);

/* Swerve Stop -- Pressing the button completely stops the robot's motion. */
driveController.getSwerveStop().onTrue(new SwerveStopCommand(swerveSubsystem));

driveController.getShooterAimButton().onTrue(Commands.runOnce(
() -> {
swerveSubsystem.setTargetPoint(fmsSubsystem.isRedAlliance() ? SwerveConstants.RED_SPEAKER_POS : SwerveConstants.BLUE_SPEAKER_POS);
swerveSubsystem.setAim(true);
}
));

driveController.getShooterAimButton().onFalse(Commands.runOnce(
() -> {
swerveSubsystem.setAim(false);
}
));



/* SHOOTER PIVOT TEST */

// rightBumper.onTrue(new ShooterPivotSetAngleCommand(shooterPivotSubsystem,
Expand Down Expand Up @@ -487,8 +527,7 @@ private void configureBindings() {
} else {
shooterPivotSubsystem.setAutoAimBoolean(false);
if (mechController.getPOV() == 90) {
shooterPivotSubsystem.setAngle(Units.degreesToRadians(60));
shooterFlywheelSubsystem.setShooterMotorSpeed(.4);

if (shooterFlywheelSubsystem.atSpeed()) {
mechController.setRumble(RumbleType.kBothRumble, .4);
} else {
Expand Down Expand Up @@ -516,6 +555,8 @@ private void configureBindings() {
}, shooterFlywheelSubsystem
));

dPadRight.onTrue(new ShooterFlywheelShuttleCommand(swerveSubsystem, shooterFlywheelSubsystem, fmsSubsystem, shooterPivotSubsystem, .6).onlyWhile(dPadRight));

// intakePivotSubsystem.setDefaultCommand(new InstantCommand(() -> {
// intakePosition = MathUtil.clamp(intakePosition, 0, 1);
// intakePivotSubsystem.setPosition(intakePosition);
Expand Down Expand Up @@ -544,36 +585,7 @@ private void configureBindings() {
() -> shooterPivotSubsystem.setAngleOffset(Units.degreesToRadians(0)))
);

shuttleNotes.onTrue(new ShooterFlywheelShuttleCommand(swerveSubsystem,
shooterFlywheelSubsystem, swerveSubsystem::getRobotPosition, shooterPivotSubsystem)
);

/* SWERVE BINDINGS */

/* Shooter Aim -- Holding down the button will change the shooter's pitch to aim it at the speaker. */
// drive

/* Amp Align -- Pressing and holding the button will cause the robot to automatically path find to the amp.
* Releasing the button will stop the robot (and the path finding). */
driveController.getAmpAlign().onTrue(new InstantCommand(
() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.AUTO_ALIGN, 1)
).andThen(new ParallelRaceGroup(
AlignCommand.getAmpAlignCommand(swerveSubsystem, fmsSubsystem.isRedAlliance()),
new ConditionalWaitCommand(
() -> !driveController.getAmpAlign().getAsBoolean()))
).andThen(new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1)))
);

/* Note align -- Auto-intakes the nearest visible note, leaving left power control to the driver. */
driveController.getNoteAlign().onTrue(
new AutoIntakeSequence(intakeRollerSubsystem, intakePivotSubsystem,
swerveSubsystem, noteDetector,
driveController, lightBarSubsystem)
.onlyWhile(driveController.getNoteAlign())
);

/* Swerve Stop -- Pressing the button completely stops the robot's motion. */
driveController.getSwerveStop().onTrue(new SwerveStopCommand(swerveSubsystem));



}
Expand Down
11 changes: 10 additions & 1 deletion src/main/java/frc/robot/commands/auton/SetCalculatedAngleCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants.SwerveConstants;
import frc.robot.subsystems.swerve.SwerveSubsystem;

/** SetCalculatedAngleCommand. */
Expand All @@ -12,11 +13,18 @@ public class SetCalculatedAngleCommand extends Command {
public SetCalculatedAngleCommand(SwerveSubsystem swerve) {
this.swerve = swerve;
addRequirements(swerve);

}

@Override
public void initialize() {
swerve.targetSpeaker();
swerve.setAim(true);
}

@Override
public void execute() {
swerve.setSwerveAimDrivePowers(0, 0);
swerve.setDrivePowers(0, 0, 0);
}

@Override
Expand All @@ -27,5 +35,6 @@ public boolean isFinished() {
@Override
public void end(boolean interrupted) {
swerve.setRobotRelativeDrivePowers(0, 0, 0);
swerve.setAim(false);
}
}
63 changes: 18 additions & 45 deletions src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelShuttleCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,10 @@

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants.ShooterConstants;
import frc.robot.subsystems.FieldManagementSubsystem;
import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem;
import frc.robot.subsystems.shooter.ShooterPivotSubsystem;
import frc.robot.subsystems.swerve.SwerveSubsystem;
Expand All @@ -15,79 +16,51 @@ public class ShooterFlywheelShuttleCommand extends Command {

private static final double MAX_ROTATION_POWER = 0.3;
private static final double ERROR_MULTIPLIER = 0.08;
private static final double TARGET_ANGLE = Math.toRadians(30);
private static final double TARGET_ANGLE = Units.degreesToRadians(70);

private final Translation2d BLUE_SHUTTLE_POINT = new Translation2d(Units.inchesToMeters(0), Units.inchesToMeters(323));
private final Translation2d RED_SHUTTLE_POINT = new Translation2d(Units.inchesToMeters(652.73), Units.inchesToMeters(323));

private final SwerveSubsystem swerveSubsystem;
private final ShooterFlywheelSubsystem flywheelSubsystem;
private Pose2dSupplier poseSupplier; //new Pose2d();
private final ShooterPivotSubsystem pivotSubsystem;
private final FieldManagementSubsystem fmsSubsystem;
private final double shooterSpeed;


private boolean redAlliance = true;
private double angleOffset;

/** Constructor for Shuttle Command. */
public ShooterFlywheelShuttleCommand(SwerveSubsystem swerveSubsystem,
ShooterFlywheelSubsystem flywheelSubsystem, Pose2dSupplier poseSupplier,
ShooterPivotSubsystem pivotSubsystem) {
ShooterFlywheelSubsystem flywheelSubsystem, FieldManagementSubsystem fmsSubsystem,
ShooterPivotSubsystem pivotSubsystem, double speed) {

this.swerveSubsystem = swerveSubsystem;
this.flywheelSubsystem = flywheelSubsystem;
this.poseSupplier = poseSupplier;
this.pivotSubsystem = pivotSubsystem;
this.fmsSubsystem = fmsSubsystem;
this.shooterSpeed = speed;

addRequirements(swerveSubsystem, flywheelSubsystem);
addRequirements(flywheelSubsystem, pivotSubsystem);
}


private double getAngle(double targetX, double targetY, double robotX, double robotY) {
return Math.atan((targetY - robotY) / (targetX - robotX));
}

/** Runs command on init. */
public void initialize() {
Pose2d currentField = poseSupplier.getPose2d();

//red alliance aim pos (652.73-114, 323-96)
double redAllianceX = Units.inchesToMeters(652.73 - 114);
double redAllianceY = Units.inchesToMeters(323 - 96);

//blue alliance aim pos (114, 323-96)
double blueAllianceX = Units.inchesToMeters(114);
double blueAllianceY = Units.inchesToMeters(323 - 96);

double robotX = currentField.getX();
double robotY = currentField.getY();
double robotAngle = currentField.getRotation().getDegrees();

if (redAlliance) {
angleOffset = getAngle(redAllianceX, redAllianceY, robotX, robotY) - robotAngle;
} else {
angleOffset = 180 - getAngle(blueAllianceX, blueAllianceY, robotX, robotY) - robotAngle;
}

System.out.println("Angle Offset is: " + angleOffset);
}

/** Is robot in center line. */
public boolean isFinished() {

return (poseSupplier.getPose2d().getX() < 326.365 + 70 || poseSupplier.getPose2d().getX() > 326.36 - 70);
swerveSubsystem.setTargetPoint(fmsSubsystem.isRedAlliance() ? RED_SHUTTLE_POINT : BLUE_SHUTTLE_POINT);
swerveSubsystem.setAim(true);
}

/** What the command has to execute. */
public void execute() {

swerveSubsystem.setRobotRelativeDrivePowers(0, 0, -MathUtil.clamp(
angleOffset * ERROR_MULTIPLIER, -MAX_ROTATION_POWER, MAX_ROTATION_POWER)); //CHANGE ANGULAR POWER
flywheelSubsystem.setShooterMotorSpeed(ShooterConstants.FLYWHEEL_SHUTTLE_SPEED);
flywheelSubsystem.setShooterMotorSpeed(shooterSpeed);
pivotSubsystem.setAngle(TARGET_ANGLE);

}

/** Run after command has ended. Turns of shooter and swerve speed.*/
public void end(boolean interrupted) {
swerveSubsystem.setRobotRelativeDrivePowers(0, 0, 0);
flywheelSubsystem.stopShooter();
swerveSubsystem.setAim(false);

System.out.println("Ended shuttle command");
}
Expand Down
9 changes: 5 additions & 4 deletions src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.ShooterConstants;
import frc.robot.Constants.SwerveConstants;
import frc.robot.util.Pose2dSupplier;

import java.util.function.BooleanSupplier;
Expand Down Expand Up @@ -214,13 +215,13 @@ public double getShootingDistance() {
Pose2d currentField = poseSupplier.getPose2d();

if (redSupplier.getAsBoolean()) { //true = red
double xLength = Math.pow(currentField.getX() - ShooterConstants.RED_X, 2);
double yLength = Math.pow(currentField.getY() - ShooterConstants.RED_Y, 2);
double xLength = Math.pow(currentField.getX() - SwerveConstants.RED_SPEAKER_POS.getX(), 2);
double yLength = Math.pow(currentField.getY() - SwerveConstants.RED_SPEAKER_POS.getY(), 2);

currentDistance = Math.sqrt(xLength + yLength);
} else {
double xLength = Math.pow(currentField.getX() - ShooterConstants.BLUE_X, 2);
double yLength = Math.pow(currentField.getY() - ShooterConstants.BLUE_Y, 2);
double xLength = Math.pow(currentField.getX() - SwerveConstants.BLUE_SPEAKER_POS.getX(), 2);
double yLength = Math.pow(currentField.getY() - SwerveConstants.BLUE_SPEAKER_POS.getY(), 2);

currentDistance = Math.sqrt(xLength + yLength);
}
Expand Down
9 changes: 5 additions & 4 deletions src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.ShooterConstants;
import frc.robot.Constants.SwerveConstants;
import frc.robot.util.Pose2dSupplier;

import java.util.function.BooleanSupplier;
Expand Down Expand Up @@ -133,13 +134,13 @@ private double getShootingDistance() {
Pose2d currentField = poseSupplier.getPose2d();

if (redSupplier.getAsBoolean()) { //true = red
double xLength = Math.pow(currentField.getX() - ShooterConstants.RED_X, 2);
double yLength = Math.pow(currentField.getY() - ShooterConstants.RED_Y, 2);
double xLength = Math.pow(currentField.getX() - SwerveConstants.RED_SPEAKER_POS.getX(), 2);
double yLength = Math.pow(currentField.getY() - SwerveConstants.RED_SPEAKER_POS.getY(), 2);

currentDistance = Math.sqrt(xLength + yLength);
} else {
double xLength = Math.pow(currentField.getX() - ShooterConstants.BLUE_X, 2);
double yLength = Math.pow(currentField.getY() - ShooterConstants.BLUE_Y, 2);
double xLength = Math.pow(currentField.getX() - SwerveConstants.BLUE_SPEAKER_POS.getX(), 2);
double yLength = Math.pow(currentField.getY() - SwerveConstants.BLUE_SPEAKER_POS.getY(), 2);

currentDistance = Math.sqrt(xLength + yLength);
}
Expand Down
Loading

0 comments on commit 3b43676

Please sign in to comment.