Added ultrashooter command and support methods, constants, controller activation
This commit is contained in:
Scott Jackson
parent
5986bc8f92
commit
1dd99e305f
|
|
@ -14,6 +14,8 @@ import edu.wpi.first.math.geometry.Transform2d;
|
|||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
|
||||
import java.util.Map;
|
||||
|
||||
/**
|
||||
* The Constants class provides a convenient place for teams to hold robot-wide
|
||||
* numerical or boolean constants. This class should not be used for any other
|
||||
|
|
@ -141,10 +143,27 @@ public final class Constants {
|
|||
public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
|
||||
public static final double HOMING_SPEED = .05;
|
||||
public static final double TURRET_GEAR_RATIO = 6;
|
||||
public static final double TURRET_RANGE_ROTATIONS = 0.5; // The turret can rotate 180°
|
||||
|
||||
public static final int HOOD_SERVO_CHANNEL = 0;
|
||||
public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this
|
||||
public static final int HOOD_ENCODER_ID = 4;
|
||||
public static final int HOOD_ANGLE_MIN_DEG = 52; // Lowest possible position
|
||||
public static final int HOOD_ANGLE_MAX_DEG = 70; // Highest possible position
|
||||
public static final int HOOD_ANGLE_PASSING_DEG = 55; // Default hood angle for passing
|
||||
public static final int HOOD_ANGLE_SCORING_MAX_DEG = 70; // Highest angle for scoring
|
||||
public static final int HOOD_ANGLE_SCORING_MIN_DEG = 62; // Lowest angle for scoring
|
||||
public static final double SCORING_DISTANCE_LONG_IN = 225; // Shortest scoring distance in inches used for finding hood angles
|
||||
public static final double SCORING_DISTANCE_SHORT_IN = 60; // Shortest scoring distance in inches used for finding hood angles
|
||||
public static final double SCORING_DISTANCE_FIELD_MAX_METERS = 7; // Sanity check value. Max distance from hub in Alliance Zone is ~6 meters
|
||||
public static final double SCORING_DISTANCE_FIELD_MIN_METERS = 0.1; // Sanity check value.
|
||||
public static final double SCORING_DISTANCE_MIN_ALLOWED_METERS = 1.5; // The minimum distance from the hub we'll allow the robot to shoot from
|
||||
public static final double SCORING_DISTANCE_MAX_ALLOWED_METERS = 7.0; // The maximum distance from the hub we'll allow the robot to shoot from
|
||||
// This is the distance increment in inches for adjusting hood angle according to distance from the target, in inches per degree:
|
||||
public static final double HOOD_ANGLE_CALC_DIVISION = (SCORING_DISTANCE_LONG_IN - SCORING_DISTANCE_SHORT_IN) / (HOOD_ANGLE_SCORING_MAX_DEG - HOOD_ANGLE_SCORING_MIN_DEG);
|
||||
public static final double SHOOTER_EFFICIENCY_GENERAL = 0.30; // This is the baseline shooter efficiency assumption
|
||||
public static final double SHOOTER_FLYWHEEL_DIAMETER_IN = 3.0; // Flywheel diameter is 3.0 inches
|
||||
public static final double SHOOTER_FLYWHEEL_GEAR_RATIO = 2.0; // Flywheel moves at twice the RPM of the flywheel motor
|
||||
public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1);
|
||||
public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90);
|
||||
public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0);
|
||||
|
|
@ -153,6 +172,41 @@ public final class Constants {
|
|||
public static final double DEFLECTOR_SET_ANGLE2 = 200;
|
||||
public static final double DEFLECTOR_SET_ANGLE3 = 300;
|
||||
public static final double DEFLECTOR_SERVO_RATIO = 1.0;
|
||||
|
||||
// Below are sets of quadratic coefficients that are to be used with each valid hood angle, for scoring and passing.
|
||||
// These are from Table 2-2 in the AutoShooter document for use in scoring when distance is in meters.
|
||||
record QuadraticCoef(double a, double b, double c) {} // Creates a basic record type with no special methods ("{}")
|
||||
|
||||
public static Map<Integer, QuadraticCoef> SCORING_SPEED_COEFS_METERS = Map.of( // Access this map to get scoring coefficients for each hood angle
|
||||
62, new QuadraticCoef(-0.08267, 3.41207, 12.20000),
|
||||
63, new QuadraticCoef(-0.12400, 3.85827, 11.50000),
|
||||
64, new QuadraticCoef(-0.09300, 3.74016, 11.70000),
|
||||
65, new QuadraticCoef(-0.06200, 3.54331, 12.40000),
|
||||
66, new QuadraticCoef(-0.09300, 3.89764, 12.00000),
|
||||
67, new QuadraticCoef(-0.06200, 3.85827, 12.20000),
|
||||
68, new QuadraticCoef(0.00000, 3.54331, 13.00000),
|
||||
69, new QuadraticCoef(0.03100, 3.58268, 13.10000),
|
||||
70, new QuadraticCoef(0.12056, 3.25022, 13.96667));
|
||||
|
||||
// These are from Table 2-2 in the AutoShooter document for use in scoring when distance is in inches.
|
||||
public static Map<Integer, QuadraticCoef> SCORING_SPEED_COEFS_INCHES = Map.of( // Access this map to get scoring coefficients for each hood angle
|
||||
62, new QuadraticCoef(-0.00005, 0.08667, 12.20000),
|
||||
63, new QuadraticCoef(-0.00008, 0.09800, 11.50000),
|
||||
64, new QuadraticCoef(-0.00006, 0.09500, 11.70000),
|
||||
65, new QuadraticCoef(-0.00004, 0.09000, 12.40000),
|
||||
66, new QuadraticCoef(-0.00006, 0.09900, 12.00000),
|
||||
67, new QuadraticCoef(-0.00004, 0.09800, 12.20000),
|
||||
68, new QuadraticCoef(0.00000, 0.09000, 13.00000),
|
||||
69, new QuadraticCoef(0.00002, 0.09100, 13.10000),
|
||||
70, new QuadraticCoef(0.00008, 0.08256, 13.96667));
|
||||
|
||||
// These are from Table 2-1 in the AutoShooter document for use in passing when distance is in meters.
|
||||
public static Map<Integer, QuadraticCoef> PASSING_SPEED_COEFS_METERS = Map.of(
|
||||
55, new QuadraticCoef(-0.09751, 3.47134, 7.8890));
|
||||
|
||||
// These are from Table 2-1 in the AutoShooter document for use in passing when distance is in inches.
|
||||
public static Map<Integer, QuadraticCoef> PASSING_SPEED_COEFS_INCHES = Map.of(
|
||||
55, new QuadraticCoef(-0.00006, 0.08817, 7.889));
|
||||
}
|
||||
|
||||
public static class HopperConstants {
|
||||
|
|
@ -187,7 +241,7 @@ public final class Constants {
|
|||
|
||||
}
|
||||
|
||||
// These represent the dividing lines for the regions in our shooting strategy.
|
||||
// These represent the dividing lines for the regions in our shooting strategy. These values are in meters.
|
||||
public static class ShootingRegionDimensions {
|
||||
|
||||
public static double BLUE_ALLIANCE_ZONE_REGION_X_MAX = 3.5;
|
||||
|
|
|
|||
|
|
@ -16,6 +16,7 @@ import frc.robot.commands.AutoTurretCommand;
|
|||
import frc.robot.commands.AutoTurretShootCommand;
|
||||
import frc.robot.commands.Reverse;
|
||||
import frc.robot.commands.SetDeflectorCommand;
|
||||
import frc.robot.commands.UltraShooterCommand;
|
||||
import frc.robot.subsystems.DriveSubsystem;
|
||||
import frc.robot.subsystems.HopperSubsystem;
|
||||
import frc.robot.subsystems.IntakeSubsystem;
|
||||
|
|
@ -59,7 +60,9 @@ public class RobotContainer {
|
|||
NamedCommands.registerCommand("RunIntake", intakeSubsystem.intake(m_driverController.getHID()));
|
||||
NamedCommands.registerCommand("TurretShoot", new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem));
|
||||
|
||||
turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
|
||||
// The default command below is commented out so that UltraShooterCommand can control turret while the shooter function is active.
|
||||
// The default command can be re-instated if we choose to track a target at all times instead.
|
||||
// turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -74,6 +77,7 @@ public class RobotContainer {
|
|||
*
|
||||
*/
|
||||
private void configureBindings() {
|
||||
// These let the driver set the shooter hood position using the controller.
|
||||
// m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
|
||||
// m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1));
|
||||
// m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
|
||||
|
|
@ -83,8 +87,10 @@ public class RobotContainer {
|
|||
m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem));
|
||||
m_driverController.rightBumper().onTrue(Commands.parallel(intakeSubsystem.intake(m_driverController.getHID())));
|
||||
m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
|
||||
m_driverController.a().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
|
||||
m_driverController.x().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
|
||||
// m_driverController.a().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
|
||||
// m_driverController.x().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
|
||||
//m_driverController.x().toggleOnTrue(getAutonomousCommand())((new UltraShooterCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
|
||||
m_driverController.x().toggleOnTrue(new UltraShooterCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem));
|
||||
m_driverController.b().whileTrue(new Reverse(intakeSubsystem, hopperSubsystem, m_driverController.getHID()));
|
||||
m_driverController.y().onTrue(intakeSubsystem.deploy());
|
||||
}
|
||||
|
|
|
|||
|
|
@ -7,9 +7,13 @@ import frc.robot.Constants.ShootingRegionDimensions;
|
|||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
|
||||
import java.util.Map;
|
||||
|
||||
public class Utils {
|
||||
|
||||
/**
|
||||
|
|
@ -37,8 +41,8 @@ public class Utils {
|
|||
/**
|
||||
* Determines the region for the robot based on its location and alliance.
|
||||
* Assumes the robot is a 27" square. Thresholds based on observations from simulator.
|
||||
* @param robotPose: the robot's pose
|
||||
* @param alliance: the red/blue alliance of the robot, as a sanity check
|
||||
* @param robotPose the robot's pose
|
||||
* @param alliance the red/blue alliance of the robot, as a sanity check
|
||||
* @return the robot's shooting region
|
||||
*/
|
||||
public static ShootingRegion findRobotShootingRegion(Pose3d robotPose, Alliance alliance){
|
||||
|
|
@ -46,7 +50,7 @@ public class Utils {
|
|||
double robotX = robotPose.getX();
|
||||
double robotY = robotPose.getY();
|
||||
|
||||
// Receommended: Add a sanity check for invalid robot poses.
|
||||
// Recommended: Add a sanity check for invalid robot poses.
|
||||
|
||||
if (alliance == Alliance.Blue) {
|
||||
if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX) {
|
||||
|
|
@ -100,6 +104,39 @@ public class Utils {
|
|||
return region;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Calculates the ideal hood angle based on the provided distance
|
||||
* between the robot's turret and the target (hub).
|
||||
* Implements the equation to find hood angle for scoring:
|
||||
* Angle = MaxAngle - ((Distance - ShortDistance) / Div), where
|
||||
* MaxAngle = maximum hood angle for scoring
|
||||
* Distance = distance to target
|
||||
* ShortDistance = shortest distance used when evaluating hood angles
|
||||
* LongDistance = longest distance used when evaluating hood angles
|
||||
* Div = the distance from the target that requires 1° of hood angle change
|
||||
* = (LongDistance - ShortDistance) / (MaxAngle - MinAngle)
|
||||
* These were all defined as constants in Constants.java.
|
||||
* @param distanceInMeters distance between turret and target, in meters
|
||||
* @return the ideal nearest-integer hood angle in degrees relative to the floor
|
||||
*/
|
||||
public static int getScoringHoodAngleDegForDistance(double distanceInMeters){
|
||||
double calculatedAngleDeg = Constants.ShooterConstants.HOOD_ANGLE_PASSING_DEG; // init to the passing value
|
||||
double distanceInInches = Units.metersToInches(distanceInMeters); // Convert to inches to match theory equations
|
||||
|
||||
// Limit the distance according to the min and max that were used in the theory calculations
|
||||
MathUtil.clamp(distanceInInches, Constants.ShooterConstants.SCORING_DISTANCE_SHORT_IN, Constants.ShooterConstants.SCORING_DISTANCE_LONG_IN);
|
||||
|
||||
// Perfom the main calculation (Equation 2-5 in the AutoShooter document), then round the result to the nearest degree
|
||||
calculatedAngleDeg = Constants.ShooterConstants.HOOD_ANGLE_SCORING_MAX_DEG - (((distanceInInches) - Constants.ShooterConstants.SCORING_DISTANCE_SHORT_IN) /
|
||||
Constants.ShooterConstants.HOOD_ANGLE_CALC_DIVISION);
|
||||
int hoodAngleDeg = (int) Math.round(calculatedAngleDeg);
|
||||
|
||||
return (hoodAngleDeg);
|
||||
}
|
||||
|
||||
|
||||
|
||||
public static double getHoodAngleForDistance(double distanceInMeters){
|
||||
if(distanceInMeters >= 4){
|
||||
return 200;
|
||||
|
|
@ -107,6 +144,93 @@ public class Utils {
|
|||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Calculates the appropriate launch speed in ft/s for scoring based on
|
||||
* the provided hood angle and target distance in meters.
|
||||
* This is done based on a series of quadratic regressions
|
||||
* See the AutoShooter theory document.
|
||||
* @param hoodAngleDeg the hood angle that the calculation should assume
|
||||
* @param distanceInMeters distance between turret and target, in meters
|
||||
* @return the appropriate launch speed for scoring in feet per second
|
||||
* Returns 0 if a launch speed cannot be determined.
|
||||
*/
|
||||
public static double findLaunchSpeedScoring(int hoodAngleDeg, double distanceInMeters){
|
||||
// Check that the requested hood angle is in the expected shooting range
|
||||
if ((hoodAngleDeg < Constants.ShooterConstants.HOOD_ANGLE_SCORING_MIN_DEG) ||
|
||||
(hoodAngleDeg > Constants.ShooterConstants.HOOD_ANGLE_SCORING_MAX_DEG)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Check that the requested distance is reasonable. If the scoring distance is outside of this range, there was a problem.
|
||||
if ((distanceInMeters < Constants.ShooterConstants.SCORING_DISTANCE_FIELD_MIN_METERS) ||
|
||||
(distanceInMeters > Constants.ShooterConstants.SCORING_DISTANCE_FIELD_MAX_METERS)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Get the quadratic coefficients for the hood angle. Return with the error value if none are found.
|
||||
Constants.ShooterConstants.QuadraticCoef coefficients = Constants.ShooterConstants.SCORING_SPEED_COEFS_METERS.get(hoodAngleDeg);
|
||||
if (coefficients == null) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Perform the quadratic calculation using the coefficients we retrieved if successful
|
||||
double x = distanceInMeters; // Rename to "x" just to make the next expression simpler
|
||||
double launchSpeedFPS = (coefficients.a() * x * x) + (coefficients.b() * x) + coefficients.c();
|
||||
|
||||
return launchSpeedFPS;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the appropriate launch speed in ft/s for passing based on
|
||||
* the provided hood angle and target distance in meters.
|
||||
* This is done based on a series of quadratic regressions
|
||||
* See the AutoShooter theory document.
|
||||
* @param hoodAngleDeg the hood angle that the calculation should assume
|
||||
* @param distanceInMeters distance between turret and target, in meters
|
||||
* @return the appropriate launch speed for passing in feet per second
|
||||
* Returns 0 if a launch speed cannot be determined.
|
||||
*/
|
||||
public static double findLaunchSpeedPassing(int hoodAngleDeg, double distanceInMeters){
|
||||
// Check that the requested hood angle is in the expected angle range for passing.
|
||||
if (hoodAngleDeg != Constants.ShooterConstants.HOOD_ANGLE_PASSING_DEG) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Check that the distance is reasonable. If the scoring distance is outside of this range, there was a problem.
|
||||
if ((distanceInMeters < Constants.ShooterConstants.SCORING_DISTANCE_FIELD_MIN_METERS) ||
|
||||
(distanceInMeters > Constants.ShooterConstants.SCORING_DISTANCE_FIELD_MAX_METERS)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Get the quadratic coefficients for the hood angle. Return with the error value if none are found.
|
||||
Constants.ShooterConstants.QuadraticCoef coefficients = Constants.ShooterConstants.PASSING_SPEED_COEFS_METERS.get(hoodAngleDeg);
|
||||
if (coefficients == null) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Perform the quadratic calculation using the coefficients we retrieved if successful
|
||||
double x = distanceInMeters; // Rename to "x" just to make the next expression simpler
|
||||
double launchSpeedFPS = (coefficients.a() * x * x) + (coefficients.b() * x) + coefficients.c();
|
||||
|
||||
return launchSpeedFPS;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the flywheel RPM that's required to achieve the provided
|
||||
* launch speed in feet/second.
|
||||
* This performs Equation 2-3 in the AutoShooter document.
|
||||
* @param launchSpeedFPS the goal launch speed in ft/s
|
||||
* @return the appropriate flywheel speed in RPM
|
||||
*/
|
||||
public static double GetShooterRPMForLaunchSpeed(double launchSpeedFPS){
|
||||
double flywheelDiameterFeet = Constants.ShooterConstants.SHOOTER_FLYWHEEL_DIAMETER_IN / 12; // Change to feet to match launch speed units
|
||||
double efficiency = Constants.ShooterConstants.SHOOTER_EFFICIENCY_GENERAL; // Use the appropriate efficiency
|
||||
|
||||
double shooterRPM = (launchSpeedFPS / (efficiency * flywheelDiameterFeet * Math.PI)) * 60; // Perform the calculation
|
||||
return shooterRPM;
|
||||
}
|
||||
|
||||
public static double getLauncherRPMForDistance(double distanceInMeters){
|
||||
return Math.min((667.557*distanceInMeters) + 1500.699, 5000);
|
||||
}
|
||||
|
|
@ -117,8 +241,8 @@ public class Utils {
|
|||
* Scoring hub, 2x passing from Neutral Zone, 2x passing from opponent's Alliance Zone,
|
||||
* and these exist for both red and blue alliances.
|
||||
* Right/left are from driver's perspective.
|
||||
* @param region: the identifier of the robot's current field region
|
||||
* @param alliance: the red/blue alliance of the robot, as a sanity check
|
||||
* @param region the identifier of the robot's current field region
|
||||
* @param alliance the red/blue alliance of the robot, as a sanity check
|
||||
* @return the shooting target for a robot with the provided properties. Returns all-zero
|
||||
* Pose3d object if the region/alliance combination is invalid.
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -0,0 +1,208 @@
|
|||
package frc.robot.commands;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
import edu.wpi.first.units.Units;
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import frc.robot.Constants;
|
||||
import frc.robot.Constants.ShootingRegion;
|
||||
import frc.robot.Utils;
|
||||
import frc.robot.subsystems.DriveSubsystem;
|
||||
import frc.robot.subsystems.HopperSubsystem;
|
||||
import frc.robot.subsystems.IntakeSubsystem;
|
||||
import frc.robot.subsystems.TurretSubsystem;
|
||||
|
||||
|
||||
public class UltraShooterCommand extends Command{
|
||||
|
||||
private HopperSubsystem hopperSubsystem;
|
||||
private TurretSubsystem turretSubsystem;
|
||||
private IntakeSubsystem intakeSubsystem; // TODO: is this necessary?
|
||||
private DriveSubsystem driveSubsystem;
|
||||
|
||||
private Command waitCommand = Commands.waitSeconds(1.5);
|
||||
private final Translation2d CENTER_TURRET_TO_ROBOT = new Translation2d(-0.1335024, -0.1824736); // Defines physical offset of turret from robot center
|
||||
|
||||
private Alliance alliance; // Shooting behavior is based on the robot's alliance
|
||||
|
||||
public UltraShooterCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem, DriveSubsystem driveSubsystem){
|
||||
this.hopperSubsystem = hopperSubsystem;
|
||||
this.turretSubsystem = turretSubsystem;
|
||||
this.intakeSubsystem = intakeSubsystem;
|
||||
this.driveSubsystem = driveSubsystem;
|
||||
|
||||
addRequirements(hopperSubsystem, intakeSubsystem); // TODO: isintakeSubsystem necessary?
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
hopperSubsystem.spindexerOn(); // Activate spindexer so it's on while command is active
|
||||
hopperSubsystem.stopKicker(); // Make sure kicker is off when command is called
|
||||
CommandScheduler.getInstance().schedule(waitCommand); // Kick off a wait timer so robot doesn't shoot until hopper is ready
|
||||
alliance = DriverStation.getAlliance().get(); // Fetch our alliance
|
||||
// Logger.recordOutput("AutoTurretShoot-State", "SHOOTING");
|
||||
Logger.recordOutput("UltraShooter-State", "ACTIVE");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
Boolean okToShoot = false; // Tracks whether conditions are OK for shooting.
|
||||
Boolean scoring = false; // Are we shooting into the hub?
|
||||
Pose3d botPose = driveSubsystem.getBotPose(); // Find the pose once since we'll use it a few times
|
||||
|
||||
// STEPS 1 & 2: Get the field location of the initial target
|
||||
// First, find the robot's current field location.
|
||||
ShootingRegion shootingRegion = Utils.findRobotShootingRegion(botPose, alliance);
|
||||
|
||||
// If robot is in a valid scoring or passing region, find the field pose for the appropriate target and perform calculations
|
||||
if(shootingRegion != ShootingRegion.NON_SHOOTING_REGION){
|
||||
|
||||
okToShoot = true; // So far, so good
|
||||
|
||||
// Check whether we're scoring, since it require some extra steps
|
||||
if ((shootingRegion == ShootingRegion.BLUE_OWN_ALLIANCE_ZONE) || (shootingRegion == ShootingRegion.RED_OWN_ALLIANCE_ZONE)){
|
||||
scoring = true;
|
||||
}
|
||||
|
||||
Pose3d initialTargetPose = Utils.findShootingTarget(shootingRegion, alliance); // Get initial target based on robot location
|
||||
Logger.recordOutput("UltraShooter-InitialTarget", initialTargetPose);
|
||||
|
||||
// STEPS 3 & 4: calculate hood and flywheel values based on distance to the intiial target.
|
||||
// First, find the turret's pose based on the robot's pose and the location of the turret relative to the robot's center.
|
||||
// Note that the turret points backward, so its rotational property is +0.5 rotations.
|
||||
Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT),
|
||||
new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative() + 0.5))));
|
||||
// Then calculate the linear distance between the initial target and the robot's turret.
|
||||
double distanceToInitialTargetMeters = initialTargetPose.toPose2d().getTranslation().getDistance(turretPose.getTranslation().toTranslation2d());
|
||||
Logger.recordOutput("UltraShooter-DistanceToInitialTarget(Meters)", distanceToInitialTargetMeters);
|
||||
|
||||
// STEP 3: Find the best hood angle
|
||||
// Default to the passing hood angle if we're not shooting into a hub.
|
||||
int initialHoodAngleDegrees = Constants.ShooterConstants.HOOD_ANGLE_PASSING_DEG;
|
||||
// If we're scoring, run the calculations to find the best hood angle.
|
||||
if (scoring) {
|
||||
initialHoodAngleDegrees = Utils.getScoringHoodAngleDegForDistance(distanceToInitialTargetMeters);
|
||||
}
|
||||
|
||||
// STEP 4: determine the right shooter flywheel speed
|
||||
// First, find the correct launch speed in feet/second. Note the unit changes!
|
||||
// This covers the scoring scenario then the passing scenario.
|
||||
double intitialLaunchSpeedFPS = 0; // The target launch speed in feet per second (FPS)
|
||||
if (scoring) {
|
||||
intitialLaunchSpeedFPS = Utils.findLaunchSpeedScoring(initialHoodAngleDegrees, distanceToInitialTargetMeters);
|
||||
} else {
|
||||
intitialLaunchSpeedFPS = Utils.findLaunchSpeedPassing(initialHoodAngleDegrees, distanceToInitialTargetMeters);
|
||||
}
|
||||
|
||||
// Handle error condition in the launch speed calcs
|
||||
if (intitialLaunchSpeedFPS == 0) {
|
||||
okToShoot = false;
|
||||
}
|
||||
|
||||
// STEP 5: Find final target based on the intial target and the robot's velocity.
|
||||
// THIS IS NOT IMPLEMENTED - SO ASSUME STATIONARY ROBOT.
|
||||
Pose3d finalTargetPose = initialTargetPose;
|
||||
double distanceToFinalTargetMeters = distanceToInitialTargetMeters;
|
||||
Logger.recordOutput("UltraShooter-FinalTarget", finalTargetPose);
|
||||
Logger.recordOutput("UltraShooter-DistanceToFinalTarget(Meters)", distanceToFinalTargetMeters);
|
||||
|
||||
// STEP 6: Find hood angle and launch speed for the final target.
|
||||
// THIS IS NOT IMPLEMENTED - SO ASSUME STATIONARY ROBOT:
|
||||
double finalLaunchSpeedFPS = intitialLaunchSpeedFPS;
|
||||
int finalHoodAngleDegrees = initialHoodAngleDegrees;
|
||||
|
||||
// Confirm we're at a valid scoring distance for the robot's capabilities. If not, don't shoot.
|
||||
if ((scoring) &&
|
||||
((distanceToFinalTargetMeters > Constants.ShooterConstants.SCORING_DISTANCE_MAX_ALLOWED_METERS) ||
|
||||
(distanceToFinalTargetMeters < Constants.ShooterConstants.SCORING_DISTANCE_MIN_ALLOWED_METERS))) {
|
||||
okToShoot = false;
|
||||
}
|
||||
|
||||
// STEP 7: Find the flywheel RPM needed to achieve the launch speed we just calculated.
|
||||
double finalShooterRPM = Utils.GetShooterRPMForLaunchSpeed(finalLaunchSpeedFPS);
|
||||
|
||||
// STEP 8: Command turretSubsystem to do what we need it to do for the launch speed and launch angle.
|
||||
// Adjust for the shooter flywheel-to-motor gear ratio when setting the flywheel motor speed.
|
||||
turretSubsystem.setHoodToLaunchAngle(finalHoodAngleDegrees);
|
||||
turretSubsystem.setLauncher(finalShooterRPM / Constants.ShooterConstants.SHOOTER_FLYWHEEL_GEAR_RATIO);
|
||||
|
||||
// STEP 9: Command the turretSubsystem to aim at the intended field target (azimuth control).
|
||||
// First, find the x,y translation between the field positions of the robot's turret and the final target.
|
||||
Translation2d finalTargetTranslation = finalTargetPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
|
||||
|
||||
// Find the rotations value, field-relative, between turret point and target point.
|
||||
double turretToTargetRotation = finalTargetTranslation.getAngle().getRotations();
|
||||
|
||||
// Command the turret to get to this field-relative angle.
|
||||
// Don't try to set the turret if it's outside of it's physical capability, and prevent shooting.
|
||||
if(turretSubsystem.isTurretReady() && turretSubsystem.isTurretAnglePossible(botPose, finalTargetTranslation)) {
|
||||
turretSubsystem.setTurretFieldRelative(botPose, turretToTargetRotation);
|
||||
} else {
|
||||
okToShoot = false;
|
||||
}
|
||||
|
||||
} else {
|
||||
// If we're in a non-shooting region while the shooting function is active,
|
||||
// set the hood to a low setting to get under the trench and prevent shooting.
|
||||
turretSubsystem.setHoodToLaunchAngle(Constants.ShooterConstants.HOOD_ANGLE_PASSING_DEG);
|
||||
okToShoot = false;
|
||||
}
|
||||
|
||||
// STEP 10: Run the kicker if all the calculations went ok, the turret is pointed at the target, and the motors are ready
|
||||
// Note we don't call turretSubsystem.isLauncherUpToSpeed() because it currently does nothing.
|
||||
if (waitCommand.isFinished() && (okToShoot)) {
|
||||
hopperSubsystem.feedKicker();
|
||||
} else{
|
||||
hopperSubsystem.stopKicker();
|
||||
}
|
||||
Logger.recordOutput("UltraShooter-OkToShoot", okToShoot);
|
||||
|
||||
// OPTIONAL: Activate rumble if it's not ok to shoot
|
||||
if (!okToShoot) {
|
||||
// ACTIVATE CONTROLLER RUMBLE
|
||||
} else {
|
||||
// DEACTIVATE CONTROLLER RUMBLE
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
waitCommand.cancel();
|
||||
hopperSubsystem.spindexerOff();
|
||||
hopperSubsystem.stopKicker();
|
||||
|
||||
turretSubsystem.setLauncher(0);
|
||||
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
|
||||
|
||||
Logger.recordOutput("UltraShooter-State", "INACTIVE");
|
||||
|
||||
DriverStation.getAlliance().ifPresent((aliance) -> {
|
||||
if (aliance == Alliance.Blue) {
|
||||
turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE);
|
||||
} else {
|
||||
turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE);
|
||||
}
|
||||
});
|
||||
|
||||
// turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
|
||||
// TODO: Deactivate rumble if we decided to use the rumble feature when the robot prevents shooting
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -38,6 +38,7 @@ import frc.robot.Constants;
|
|||
import frc.robot.Robot;
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.units.AngleUnit;
|
||||
import edu.wpi.first.units.Units;
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
|
|
@ -130,6 +131,14 @@ public class TurretSubsystem extends TestableSubsystem {
|
|||
turretState = TurretState.INIT;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the launcher motor controller (shooter flywheel motor) to achieve
|
||||
* the provided motor speed (in RPM) using the Spark controller's velocity control mode.
|
||||
* The resulting shooter flywheel speed is twice the motor speed.
|
||||
* @param speed the commanded motor speed in RPM
|
||||
* @param distanceInMeters distance between turret and target, in meters
|
||||
* @return nothing
|
||||
*/
|
||||
public void setLauncher(double speed) {
|
||||
if(speed == 0)
|
||||
{
|
||||
|
|
@ -146,6 +155,12 @@ public class TurretSubsystem extends TestableSubsystem {
|
|||
// return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.03;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the hood angle when the target elevation servo position is known,
|
||||
* from zero (lowest hood angle) to 300 (highest hood angle).
|
||||
* @param elev the target servo motor position, from 0 to 300
|
||||
* @return nothing
|
||||
*/
|
||||
public void setTargetElevation(double elev)
|
||||
{
|
||||
double pos = Math.abs((elev - 300)/300);
|
||||
|
|
@ -153,6 +168,31 @@ public class TurretSubsystem extends TestableSubsystem {
|
|||
hood.set(pos);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the launch (hood) angle when the target angle is known, in degrees.
|
||||
* @param angleDegrees the commanded motor speed in RPM
|
||||
* @return nothing
|
||||
*/
|
||||
public void setHoodToLaunchAngle(int angleDegrees)
|
||||
{
|
||||
// Confirm target is within expected range. If not, do nothing.
|
||||
if ((angleDegrees < Constants.ShooterConstants.HOOD_ANGLE_MIN_DEG) ||
|
||||
(angleDegrees > Constants.ShooterConstants.HOOD_ANGLE_MAX_DEG)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Grab min and max values to simplify the expressions and ensure precision
|
||||
double minAngle = (double) Constants.ShooterConstants.HOOD_ANGLE_MIN_DEG;
|
||||
double maxAngle = (double) Constants.ShooterConstants.HOOD_ANGLE_MAX_DEG;
|
||||
|
||||
// Map so that min angle maps to "1" and max angle maps to "0."
|
||||
double pos = (maxAngle - angleDegrees) / (maxAngle - minAngle);
|
||||
|
||||
Logger.recordOutput("ShooterHood-Angle", pos);
|
||||
hood.set(pos);
|
||||
}
|
||||
|
||||
|
||||
public Command revSpeedCommand()
|
||||
{
|
||||
return this.runOnce(() -> setLauncher(-.5));
|
||||
|
|
@ -246,6 +286,13 @@ public class TurretSubsystem extends TestableSubsystem {
|
|||
|
||||
}
|
||||
|
||||
/**
|
||||
* Commands turret to point in a specific field direction given
|
||||
* the current heading of the robot.
|
||||
* @param robotPose the robot's field-relative pose
|
||||
* @param pos the field-relative rotations of angle to the target
|
||||
* @return nothing
|
||||
*/
|
||||
public void setTurretFieldRelative(Pose3d robotPose, double pos)
|
||||
{
|
||||
if(turretState != TurretState.READY)
|
||||
|
|
@ -305,6 +352,24 @@ public class TurretSubsystem extends TestableSubsystem {
|
|||
return Math.abs(turretPositionError.refresh().getValueAsDouble()) <= 0.1;
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks whether the turret can physically achieve the angle to the
|
||||
* target based on the robot's heading.
|
||||
* Uses Rotation2d to simplify the math.
|
||||
* Assumes turret rotational midpoint is 180° opposite robot's heading.
|
||||
* @param botPose the robot's field-relative pose
|
||||
* @param targetTranslation the translation from the turret to the intended target
|
||||
* @return true if angle is achieveable
|
||||
*/
|
||||
public boolean isTurretAnglePossible(Pose3d botPose, Translation2d targetTranslation){
|
||||
Rotation2d robotHeading = botPose.getRotation().toRotation2d(); // Robot's field-relative heading (forward)
|
||||
Rotation2d turretCenterPos = robotHeading.plus(Rotation2d.fromRotations(0.5)); // Turret's neutral azimuth
|
||||
Rotation2d targetDirection = targetTranslation.getAngle(); // Field-relative aiming angle
|
||||
Rotation2d turretOffset = targetDirection.minus(turretCenterPos); // Difference between the two
|
||||
|
||||
return (Math.abs(turretOffset.getRotations()) <= (Constants.ShooterConstants.TURRET_RANGE_ROTATIONS / 2));
|
||||
}
|
||||
|
||||
private double getRequestedPosTurretRelative() {
|
||||
return (turretControlMode.getPositionMeasure().in(Units.Rotations) - turretOffsetInRotations) / Constants.ShooterConstants.TURRET_GEAR_RATIO;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue