diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 789fdae..4394198 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,8 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -19,15 +21,23 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.AutoConstants; +import frc.robot.constants.CompetitionConstants; +import frc.robot.constants.HoodConstants; import frc.robot.constants.OIConstants; +import frc.robot.constants.ShooterConstants; +import frc.robot.constants.ShooterConstants.ShooterSpeeds; import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Hood; import frc.robot.subsystems.PhotonVision; +import frc.robot.subsystems.Shooter; import frc.robot.utilities.Elastic; import frc.robot.utilities.Utilities; public class RobotContainer { private PhotonVision vision; private Drivetrain drivetrain; + private Hood hood; + private Shooter shooter; private CommandXboxController driver; @@ -38,6 +48,8 @@ public class RobotContainer { public RobotContainer() { vision = new PhotonVision(); drivetrain = new Drivetrain(null); + hood = new Hood(); + shooter = new Shooter(); vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose); @@ -72,6 +84,33 @@ public class RobotContainer { false // TODO Should this be true by default? ) ); + + shooter.setDefaultCommand( + shooter.maintainSpeed(ShooterSpeeds.kHubSpeed) + ); + + hood.setDefaultCommand(hood.trackToAngle(() -> { + Pose2d drivetrainPose = drivetrain.getPose(); + Pose2d hubPose = Utilities.getHubPose(); + + double distance = drivetrainPose.getTranslation() + .plus(CompetitionConstants.KRobotToShooter.getTranslation().toTranslation2d()) + .getDistance(hubPose.getTranslation()); + + if(HoodConstants.kUseInterpolatorForAngle) { + return HoodConstants.kDistanceToAngle.get(distance); + } else { + // TODO The average actual speeds isn't really the exit velocity of the ball + // on a hooded shooter, based on documentation, it's more like 30-50% depending on + // hood material, surface friction, etc. + return Utilities.shotAngle( + shooter.getAverageActualSpeeds(), + distance, + CompetitionConstants.kHubGoalHeightMeters - ShooterConstants.kShooterHeightMeters, + false + ); + } + })); } private void configureNamedCommands() { diff --git a/src/main/java/frc/robot/constants/CompetitionConstants.java b/src/main/java/frc/robot/constants/CompetitionConstants.java index 5b515e0..290fe84 100644 --- a/src/main/java/frc/robot/constants/CompetitionConstants.java +++ b/src/main/java/frc/robot/constants/CompetitionConstants.java @@ -4,6 +4,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.util.Units; public class CompetitionConstants { @@ -14,6 +15,11 @@ public class CompetitionConstants { AprilTagFields.kDefaultField ); + public static final double kHubGoalHeightMeters = Units.inchesToMeters(72); + + // TODO Real Values + public static final Transform3d KRobotToShooter = new Transform3d(); + public static final Pose2d kBlueHubLocation = new Pose2d( Units.inchesToMeters(182.11), Units.inchesToMeters(158.84), diff --git a/src/main/java/frc/robot/constants/HoodConstants.java b/src/main/java/frc/robot/constants/HoodConstants.java index 161f7e0..f0380ec 100644 --- a/src/main/java/frc/robot/constants/HoodConstants.java +++ b/src/main/java/frc/robot/constants/HoodConstants.java @@ -4,6 +4,8 @@ import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; + public class HoodConstants { // TODO Real Values public static final int kMotorCANID = 0; @@ -15,13 +17,18 @@ public class HoodConstants { public static final double kV = 0; public static final double kA = 0; public static final double kStartupAngle = 0; + public static final double kMaxManualSpeedMultiplier = .5; public static final int kCurrentLimit = 15; public static final boolean kInverted = false; + public static final boolean kUseInterpolatorForAngle = false; public static final IdleMode kIdleMode = IdleMode.kBrake; + // TODO This needs to be filled in from some source + public static final InterpolatingDoubleTreeMap kDistanceToAngle = new InterpolatingDoubleTreeMap(); + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM public static final SparkMaxConfig kConfig = new SparkMaxConfig(); diff --git a/src/main/java/frc/robot/constants/IntakePivotConstants.java b/src/main/java/frc/robot/constants/IntakePivotConstants.java index 7fccd7b..6b2397c 100644 --- a/src/main/java/frc/robot/constants/IntakePivotConstants.java +++ b/src/main/java/frc/robot/constants/IntakePivotConstants.java @@ -21,9 +21,10 @@ public class IntakePivotConstants { } public static final int kLeftMotorCANID = 0; - public static final int kRightMotorCANID = 1; + public static final int kRightMotorCANID = 0; public static final double kConversionFactor = 0; + public static final double kMaxManualSpeedMultiplier = .5; public static final double kP = 0; public static final double kI = 0; diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index db4aa15..ae24d45 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -8,23 +8,17 @@ import edu.wpi.first.math.util.Units; public class ShooterConstants { public enum ShooterSpeeds { - kHubSpeed(0, 0), - kFeedSpeed(0, 0); + kHubSpeed(0), + kFeedSpeed(0); - private double frontRollerMPS; - private double rearRollerMPS; + private double speedMPS; - private ShooterSpeeds(double frontRollerMPS, double rearRollerMPS) { - this.frontRollerMPS = frontRollerMPS; - this.rearRollerMPS = rearRollerMPS; + private ShooterSpeeds(double speedMPS) { + this.speedMPS = speedMPS; } - public double getFrontRollerMPS() { - return frontRollerMPS; - } - - public double getRearRollerMPS() { - return rearRollerMPS; + public double getSpeedMPS() { + return speedMPS; } } @@ -33,29 +27,31 @@ public class ShooterConstants { public static final double kWheelDiameter = Units.inchesToMeters(6); // TODO Real values - public static final int kFrontShooterMotor1CANID = 0; - public static final int kFrontShooterMotor2CANID = 0; - public static final int kRearShooterMotor1CANID = 0; - public static final int kRearShooterMotor2CANID = 0; + public static final int kLeftShooterMotor1CANID = 0; + public static final int kLeftShooterMotor2CANID = 0; + public static final int kRightShooterMotor1CANID = 0; + public static final int kRightShooterMotor2CANID = 0; - public static final boolean kFrontShooterMotor1Inverted = false; - public static final boolean kFrontShooterMotor2Inverted = false; - public static final boolean kRearShooterMotor1Inverted = false; - public static final boolean kRearShooterMotor2Inverted = false; + public static final boolean kLeftShooterMotor1Inverted = false; + public static final boolean kLeftShooterMotor2Inverted = false; + public static final boolean kRightShooterMotor1Inverted = false; + public static final boolean kRightShooterMotor2Inverted = false; - public static final double kFrontP = 0; - public static final double kFrontI = 0; - public static final double kFrontD = 0; - public static final double kFrontS = 0; - public static final double kFrontV = 0; - public static final double kFrontA = 0; + public static final double kLeftP = 0; + public static final double kLeftI = 0; + public static final double kLeftD = 0; + public static final double kLeftS = 0; + public static final double kLeftV = 0; + public static final double kLeftA = 0; - public static final double kRearP = 0; - public static final double kRearI = 0; - public static final double kRearD = 0; - public static final double kRearS = 0; - public static final double kRearV = 0; - public static final double kRearA = 0; + public static final double kRightP = 0; + public static final double kRightI = 0; + public static final double kRightD = 0; + public static final double kRightS = 0; + public static final double kRightV = 0; + public static final double kRightA = 0; + + public static final double kShooterHeightMeters = 0; // TODO Is this value sane? public static final int kCurrentLimit = 30; @@ -64,51 +60,51 @@ public class ShooterConstants { // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM - public static final SparkMaxConfig kFrontMotor1Config = new SparkMaxConfig(); - public static final SparkMaxConfig kFrontMotor2Config = new SparkMaxConfig(); - public static final SparkMaxConfig kRearMotor1Config = new SparkMaxConfig(); - public static final SparkMaxConfig kRearMotor2Config = new SparkMaxConfig(); + public static final SparkMaxConfig kLeftMotor1Config = new SparkMaxConfig(); + public static final SparkMaxConfig kLeftMotor2Config = new SparkMaxConfig(); + public static final SparkMaxConfig kRightMotor1Config = new SparkMaxConfig(); + public static final SparkMaxConfig kRightMotor2Config = new SparkMaxConfig(); static { - kFrontMotor1Config + kLeftMotor1Config .idleMode(kShooterIdleMode) .smartCurrentLimit(kCurrentLimit) - .inverted(kFrontShooterMotor1Inverted); - kFrontMotor1Config.absoluteEncoder + .inverted(kLeftShooterMotor1Inverted); + kLeftMotor1Config.absoluteEncoder .positionConversionFactor(kWheelDiameter * Math.PI) .velocityConversionFactor(kWheelDiameter * Math.PI / 60); - kFrontMotor1Config.closedLoop + kLeftMotor1Config.closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) - .pid(kFrontP, kFrontI, kFrontD) + .pid(kLeftP, kLeftI, kLeftD) .outputRange(-1, 1) .feedForward - .sva(kFrontS, kFrontV, kFrontA); + .sva(kLeftS, kLeftV, kLeftA); - kFrontMotor2Config + kLeftMotor2Config .idleMode(kShooterIdleMode) .smartCurrentLimit(kCurrentLimit) - .inverted(kFrontShooterMotor2Inverted) - .follow(kFrontShooterMotor1CANID); + .inverted(kLeftShooterMotor2Inverted) + .follow(kLeftShooterMotor1CANID); - kRearMotor1Config + kRightMotor1Config .idleMode(kShooterIdleMode) .smartCurrentLimit(kCurrentLimit) - .inverted(kRearShooterMotor1Inverted); - kRearMotor1Config.absoluteEncoder + .inverted(kRightShooterMotor1Inverted); + kRightMotor1Config.absoluteEncoder .positionConversionFactor(kWheelDiameter * Math.PI) .velocityConversionFactor(kWheelDiameter * Math.PI / 60); - kRearMotor1Config.closedLoop + kRightMotor1Config.closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) - .pid(kRearP, kRearI, kRearD) + .pid(kRightP, kRightI, kRightD) .outputRange(-1, 1) .feedForward - .sva(kRearS, kRearV, kRearA); + .sva(kRightS, kRightV, kRightA); - kRearMotor2Config + kRightMotor2Config .idleMode(kShooterIdleMode) .smartCurrentLimit(kCurrentLimit) - .inverted(kRearShooterMotor2Inverted) - .follow(kRearShooterMotor1CANID); + .inverted(kRightShooterMotor2Inverted) + .follow(kRightShooterMotor1CANID); } } diff --git a/src/main/java/frc/robot/interfaces/IVisualPoseProvider.java b/src/main/java/frc/robot/interfaces/IVisualPoseProvider.java deleted file mode 100644 index 1f6cf62..0000000 --- a/src/main/java/frc/robot/interfaces/IVisualPoseProvider.java +++ /dev/null @@ -1,27 +0,0 @@ -package frc.robot.interfaces; - -import java.util.Optional; - -import edu.wpi.first.math.geometry.Pose2d; - -/** - * An interface which ensures a class' ability to provide visual pose information - * in a consistent way - */ -public interface IVisualPoseProvider { - /** - * A record that can contain the two elements necessary for a WPILIB - * pose estimator to use the information from a vision system as part of a full - * robot pose estimation - */ - public record VisualPose(Pose2d visualPose, double timestamp) {} - - /** - * Return a VisualPose or null if an empty Optional if none is available. - * Implementation should provide an empty response if it's unable to provide - * a reliable pose, or any pose at all. - * - * @return An Optional containing a VisualPose, or empty if no VisualPose can reliably be provided - */ - public Optional getVisualPose(); -} diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 98cce22..f126311 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -1,7 +1,6 @@ package frc.robot.subsystems; import java.util.Optional; -import java.util.OptionalDouble; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; @@ -30,8 +29,8 @@ import frc.robot.constants.AutoConstants; import frc.robot.constants.CompetitionConstants; import frc.robot.constants.DrivetrainConstants; import frc.robot.constants.OIConstants; -import frc.robot.interfaces.IVisualPoseProvider.VisualPose; import frc.robot.utilities.SwerveModule; +import frc.robot.utilities.VisualPose; public class Drivetrain extends SubsystemBase { private SwerveModule frontLeft; @@ -45,9 +44,6 @@ public class Drivetrain extends SubsystemBase { private PIDController yawRotationController; - private PhotonVision camera1; - private PhotonVision camera2; - public Drivetrain(Pose2d startupPose) { frontLeft = new SwerveModule( "FrontLeft", diff --git a/src/main/java/frc/robot/subsystems/Hood.java b/src/main/java/frc/robot/subsystems/Hood.java index 710bad4..4c73378 100644 --- a/src/main/java/frc/robot/subsystems/Hood.java +++ b/src/main/java/frc/robot/subsystems/Hood.java @@ -48,12 +48,18 @@ public class Hood extends SubsystemBase { }); } - public Command stop() { + public Command manualSpeed(DoubleSupplier speed) { + currentTargetRadians = 0; + return run(() -> { - motor.disable(); + motor.set(speed.getAsDouble() * HoodConstants.kMaxManualSpeedMultiplier); }); } + public Command stop() { + return manualSpeed(() -> 0); + } + public double getTargetRadians() { return currentTargetRadians; } diff --git a/src/main/java/frc/robot/subsystems/IntakePivot.java b/src/main/java/frc/robot/subsystems/IntakePivot.java index d256609..fc9c2e0 100644 --- a/src/main/java/frc/robot/subsystems/IntakePivot.java +++ b/src/main/java/frc/robot/subsystems/IntakePivot.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import java.util.Optional; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -69,8 +70,16 @@ public class IntakePivot extends SubsystemBase { }); } + public Command manualSpeed(DoubleSupplier speed) { + currentTargetPosition = null; + + return run(() -> { + leftMotor.set(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier); + }); + } + public Command stop() { - return maintainPosition(null); + return manualSpeed(() -> 0); } public Optional getCurrentTargetPosition() { diff --git a/src/main/java/frc/robot/subsystems/PhotonVision.java b/src/main/java/frc/robot/subsystems/PhotonVision.java index d90dece..77586f8 100644 --- a/src/main/java/frc/robot/subsystems/PhotonVision.java +++ b/src/main/java/frc/robot/subsystems/PhotonVision.java @@ -24,9 +24,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.CompetitionConstants; import frc.robot.constants.PhotonConstants; import frc.robot.interfaces.IAprilTagProvider; -import frc.robot.interfaces.IVisualPoseProvider; -import frc.robot.interfaces.IVisualPoseProvider.VisualPose; import frc.robot.utilities.PhotonVisionConfig; +import frc.robot.utilities.VisualPose; public class PhotonVision extends SubsystemBase { private PhotonCamera[] cameras; diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 1622e0b..8b685f3 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import java.util.Optional; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -18,54 +19,54 @@ import frc.robot.constants.ShooterConstants; import frc.robot.constants.ShooterConstants.ShooterSpeeds; public class Shooter extends SubsystemBase { - private SparkMax frontMotor1; - private SparkMax frontMotor2; - private SparkMax rearMotor1; - private SparkMax rearMotor2; + private SparkMax leftMotor1; + private SparkMax leftMotor2; + private SparkMax rightMotor1; + private SparkMax rightMotor2; - private AbsoluteEncoder frontEncoder; - private AbsoluteEncoder rearEncoder; + private AbsoluteEncoder leftEncoder; + private AbsoluteEncoder rightEncoder; - private SparkClosedLoopController frontClosedLoopController; - private SparkClosedLoopController rearClosedLoopController; + private SparkClosedLoopController leftClosedLoopController; + private SparkClosedLoopController rightClosedLoopController; private ShooterSpeeds targetSpeeds; public Shooter() { - frontMotor1 = new SparkMax(ShooterConstants.kFrontShooterMotor1CANID, MotorType.kBrushless); - frontMotor2 = new SparkMax(ShooterConstants.kFrontShooterMotor2CANID, MotorType.kBrushless); - rearMotor1 = new SparkMax(ShooterConstants.kRearShooterMotor1CANID, MotorType.kBrushless); - rearMotor2 = new SparkMax(ShooterConstants.kRearShooterMotor2CANID, MotorType.kBrushless); + leftMotor1 = new SparkMax(ShooterConstants.kLeftShooterMotor1CANID, MotorType.kBrushless); + leftMotor2 = new SparkMax(ShooterConstants.kLeftShooterMotor2CANID, MotorType.kBrushless); + rightMotor1 = new SparkMax(ShooterConstants.kRightShooterMotor1CANID, MotorType.kBrushless); + rightMotor2 = new SparkMax(ShooterConstants.kRightShooterMotor2CANID, MotorType.kBrushless); - frontMotor1.configure( - ShooterConstants.kFrontMotor1Config, + leftMotor1.configure( + ShooterConstants.kLeftMotor1Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters ); - rearMotor1.configure( - ShooterConstants.kRearMotor1Config, + rightMotor1.configure( + ShooterConstants.kRightMotor1Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters ); - frontMotor2.configure( - ShooterConstants.kFrontMotor2Config, + leftMotor2.configure( + ShooterConstants.kLeftMotor2Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters ); - rearMotor2.configure( - ShooterConstants.kRearMotor2Config, + rightMotor2.configure( + ShooterConstants.kRightMotor2Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters ); - frontEncoder = frontMotor1.getAbsoluteEncoder(); - rearEncoder = rearMotor1.getAbsoluteEncoder(); + leftEncoder = leftMotor1.getAbsoluteEncoder(); + rightEncoder = rightMotor1.getAbsoluteEncoder(); - frontClosedLoopController = frontMotor1.getClosedLoopController(); - rearClosedLoopController = rearMotor1.getClosedLoopController(); + leftClosedLoopController = leftMotor1.getClosedLoopController(); + rightClosedLoopController = rightMotor1.getClosedLoopController(); // TODO Set this to the initial startup speed targetSpeeds = null; @@ -74,21 +75,16 @@ public class Shooter extends SubsystemBase { @Override public void periodic() { Logger.recordOutput( - "Shooter/FrontRollers/TargetMPS", - targetSpeeds == null ? 0 : targetSpeeds.getFrontRollerMPS() + "Shooter/TargetMPS", + targetSpeeds == null ? 0 : targetSpeeds.getSpeedMPS() ); - Logger.recordOutput( - "Shooter/RearRollers/TargetMPS", - targetSpeeds == null ? 0 : targetSpeeds.getRearRollerMPS() - ); - - Logger.recordOutput("Shooter/FrontRollers/CurrentMPS", frontEncoder.getVelocity()); - Logger.recordOutput("Shooter/RearRollers/CurrentMPS", rearEncoder.getVelocity()); + Logger.recordOutput("Shooter/LeftRollers/CurrentMPS", leftEncoder.getVelocity()); + Logger.recordOutput("Shooter/RightRollers/CurrentMPS", rightEncoder.getVelocity()); // TODO How does the SparkMAX controller determine "at setpoint"? Is there any tolerance? - Logger.recordOutput("Shooter/FrontRollers/AtSetpoint", frontClosedLoopController.isAtSetpoint()); - Logger.recordOutput("Shooter/RearRollers/AtSetpoint", rearClosedLoopController.isAtSetpoint()); + Logger.recordOutput("Shooter/LeftRollers/AtSetpoint", leftClosedLoopController.isAtSetpoint()); + Logger.recordOutput("Shooter/RightRollers/AtSetpoint", rightClosedLoopController.isAtSetpoint()); } public Command maintainSpeed(ShooterSpeeds speeds) { @@ -96,24 +92,37 @@ public class Shooter extends SubsystemBase { return run(() -> { if(targetSpeeds == null) { - frontMotor1.disable(); - rearMotor1.disable(); + leftMotor1.disable(); + rightMotor1.disable(); } else { - frontClosedLoopController.setSetpoint( - targetSpeeds.getFrontRollerMPS(), + leftClosedLoopController.setSetpoint( + targetSpeeds.getSpeedMPS(), ControlType.kVelocity ); - rearClosedLoopController.setSetpoint( - targetSpeeds.getRearRollerMPS(), + rightClosedLoopController.setSetpoint( + targetSpeeds.getSpeedMPS(), ControlType.kVelocity ); } }); } + public Command manualSpeed(DoubleSupplier speed) { + targetSpeeds = null; + + return run(() -> { + leftMotor1.set(speed.getAsDouble()); + rightMotor1.set(speed.getAsDouble()); + }); + } + public Command stop() { - return maintainSpeed(null); + return manualSpeed(() -> 0); + } + + public double getAverageActualSpeeds() { + return (leftEncoder.getVelocity() + rightEncoder.getVelocity()) / 2; } public Optional getTargetSpeeds() { diff --git a/src/main/java/frc/robot/utilities/VisualPose.java b/src/main/java/frc/robot/utilities/VisualPose.java new file mode 100644 index 0000000..2bcc0f1 --- /dev/null +++ b/src/main/java/frc/robot/utilities/VisualPose.java @@ -0,0 +1,5 @@ +package frc.robot.utilities; + +import edu.wpi.first.math.geometry.Pose2d; + +public record VisualPose(Pose2d visualPose, double timestamp) {}