diff --git a/src/main/java/first/robot/constants/CompetitionConstants.java b/src/main/java/first/robot/constants/CompetitionConstants.java index 86c9854..1768a88 100644 --- a/src/main/java/first/robot/constants/CompetitionConstants.java +++ b/src/main/java/first/robot/constants/CompetitionConstants.java @@ -1,5 +1,6 @@ package first.robot.constants; +import org.wpilib.fields.Field; import org.wpilib.fields.Fields; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Rotation2d; @@ -7,7 +8,7 @@ import org.wpilib.math.geometry.Transform3d; import org.wpilib.math.util.Units; public class CompetitionConstants { - public static final Fields kTagLayout = Fields.FRC_2026_REBUILT_ANDY_MARK; + public static final Field kField = Fields.FRC_2026_REBUILT_ANDY_MARK.loadField(); public static final double kHubGoalHeightMeters = Units.inchesToMeters(72); diff --git a/src/main/java/first/robot/constants/DrivetrainConstants.java b/src/main/java/first/robot/constants/DrivetrainConstants.java index 6e836e9..8d0ed4e 100644 --- a/src/main/java/first/robot/constants/DrivetrainConstants.java +++ b/src/main/java/first/robot/constants/DrivetrainConstants.java @@ -1,5 +1,6 @@ package first.robot.constants; +import org.wpilib.hardware.bus.CANPort; import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.kinematics.SwerveDriveKinematics; import org.wpilib.math.linalg.Matrix; @@ -20,6 +21,8 @@ public class DrivetrainConstants { public static final double kRearLeftMagEncoderOffset = 3.761; public static final double kRearRightMagEncoderOffset = 2.573; + public static final CANPort kCANPort = CANPort.CAN_S0; + public static final int kFrontLeftDrivingCANID = 4; public static final int kFrontRightDrivingCANID = 3; public static final int kRearLeftDrivingCANID = 1; diff --git a/src/main/java/first/robot/constants/HoodConstants.java b/src/main/java/first/robot/constants/HoodConstants.java index 48b2cc9..f1547ac 100644 --- a/src/main/java/first/robot/constants/HoodConstants.java +++ b/src/main/java/first/robot/constants/HoodConstants.java @@ -2,6 +2,7 @@ package first.robot.constants; import java.util.Map; +import org.wpilib.hardware.bus.CANPort; import org.wpilib.math.interpolation.InterpolatingDoubleTreeMap; import org.wpilib.math.util.Units; @@ -14,6 +15,8 @@ import first.robot.constants.ShooterConstants.ShooterSpeeds; public class HoodConstants { // TODO Real Values + public static final CANPort kCANPort = CANPort.CAN_S0; + public static final int kMotorCANID = 12; public static final double kConversionFactor = (1.0/3.0)*(8.0/147.0)*2*Math.PI; diff --git a/src/main/java/first/robot/constants/IntakeRollerConstants.java b/src/main/java/first/robot/constants/IntakeRollerConstants.java index 378de16..4dc5e40 100644 --- a/src/main/java/first/robot/constants/IntakeRollerConstants.java +++ b/src/main/java/first/robot/constants/IntakeRollerConstants.java @@ -17,7 +17,8 @@ public class IntakeRollerConstants { public static final boolean kInvertLeftMotor = false; public static final boolean kInvertRightMotor = true; - public static final double kSpeed = 1; + public static final double kIntakeInSpeed = .9; + public static final double kIntakeOutSpeed = 1; public static final IdleMode kIdleMode = IdleMode.kCoast; diff --git a/src/main/java/first/robot/constants/ModuleConstants.java b/src/main/java/first/robot/constants/ModuleConstants.java index c2540d0..8954e44 100644 --- a/src/main/java/first/robot/constants/ModuleConstants.java +++ b/src/main/java/first/robot/constants/ModuleConstants.java @@ -1,5 +1,6 @@ package first.robot.constants; +import org.wpilib.hardware.bus.CANPort; import org.wpilib.math.util.Units; import com.ctre.phoenix6.configs.AudioConfigs; diff --git a/src/main/java/first/robot/constants/SystemCoreConstants.java b/src/main/java/first/robot/constants/SystemCoreConstants.java new file mode 100644 index 0000000..dacc45e --- /dev/null +++ b/src/main/java/first/robot/constants/SystemCoreConstants.java @@ -0,0 +1,7 @@ +package first.robot.constants; + +import org.wpilib.hardware.imu.OnboardIMU.MountOrientation; + +public class SystemCoreConstants { + public static final MountOrientation kSystemCoreMountOrientation = MountOrientation.FLAT; +} diff --git a/src/main/java/first/robot/subsystems/Drivetrain.java b/src/main/java/first/robot/subsystems/Drivetrain.java new file mode 100644 index 0000000..4b75947 --- /dev/null +++ b/src/main/java/first/robot/subsystems/Drivetrain.java @@ -0,0 +1,444 @@ +package first.robot.subsystems; + +import java.util.List; +import java.util.Optional; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import org.wpilib.command2.Command; +import org.wpilib.command2.PrintCommand; +import org.wpilib.command2.SubsystemBase; +import org.wpilib.driverstation.Alliance; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.driverstation.MatchState; +import org.wpilib.hardware.imu.OnboardIMU; +import org.wpilib.hardware.imu.OnboardIMU.MountOrientation; +import org.wpilib.math.controller.PIDController; +import org.wpilib.math.estimator.SwerveDrivePoseEstimator; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.ChassisVelocities; +import org.wpilib.math.kinematics.SwerveDriveKinematics; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleVelocity; +import org.wpilib.math.util.MathUtil; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; + +import first.robot.constants.AutoConstants; +import first.robot.constants.DrivetrainConstants; +import first.robot.constants.ModuleConstants.ModuleName; +import first.robot.constants.OIConstants; +import first.robot.constants.SystemCoreConstants; +import first.robot.utilities.SwerveModule; +import first.robot.utilities.Utilities; +import first.robot.utilities.VisualPose; + +public class Drivetrain extends SubsystemBase { + private SwerveModule frontLeft; + private SwerveModule frontRight; + private SwerveModule rearLeft; + private SwerveModule rearRight; + + private OnboardIMU imu; + + private SwerveDrivePoseEstimator estimator; + + private PIDController yawRotationController; + + public Drivetrain(Pose2d startupPose) { + frontLeft = new SwerveModule( + DrivetrainConstants.kCANPort, + ModuleName.kFrontLeft, + DrivetrainConstants.kFrontLeftDrivingCANID, + DrivetrainConstants.kFrontLeftTurningCANID, + DrivetrainConstants.kFrontLeftAnalogInPort, + DrivetrainConstants.kFrontLeftMagEncoderOffset + ); + + frontRight = new SwerveModule( + DrivetrainConstants.kCANPort, + ModuleName.kFrontRight, + DrivetrainConstants.kFrontRightDrivingCANID, + DrivetrainConstants.kFrontRightTurningCANID, + DrivetrainConstants.kFrontRightAnalogInPort, + DrivetrainConstants.kFrontRightMagEncoderOffset + ); + + rearLeft = new SwerveModule( + DrivetrainConstants.kCANPort, + ModuleName.kRearLeft, + DrivetrainConstants.kRearLeftDrivingCANID, + DrivetrainConstants.kRearLeftTurningCANID, + DrivetrainConstants.kRearLeftAnalogInPort, + DrivetrainConstants.kRearLeftMagEncoderOffset + ); + + rearRight = new SwerveModule( + DrivetrainConstants.kCANPort, + ModuleName.kRearRight, + DrivetrainConstants.kRearRightDrivingCANID, + DrivetrainConstants.kRearRightTurningCANID, + DrivetrainConstants.kRearRightAnalogInPort, + DrivetrainConstants.kRearRightMagEncoderOffset + ); + + imu = new OnboardIMU(SystemCoreConstants.kSystemCoreMountOrientation); + + yawRotationController = new PIDController( + DrivetrainConstants.kHeadingP, + 0, + 0 + ); + yawRotationController.enableContinuousInput(-Math.PI, Math.PI); + yawRotationController.setTolerance(DrivetrainConstants.kYawPIDTolerance); + + // TODO 2025 used non-standard deviations for encoder/gyro inputs and vision, will need to be tuned for 2026 in the future + estimator = new SwerveDrivePoseEstimator( + DrivetrainConstants.kDriveKinematics, + Rotation2d.fromDegrees(getGyroValue()), + new SwerveModulePosition[] { + frontLeft.getPosition(), + frontRight.getPosition(), + rearLeft.getPosition(), + rearRight.getPosition() + }, + startupPose != null ? startupPose : new Pose2d(), + DrivetrainConstants.kSensorFusionOdometryStdDevs, + DrivetrainConstants.kVisionOdometryStdDevs + ); + + if(AutoConstants.kAutoConfigOk) { + AutoBuilder.configure( + this::getPose, + this::resetOdometry, + this::getCurrentChassisSpeeds, + (speeds, feedforwards) -> driveWithChassisSpeeds(speeds), + AutoConstants.kPPDriveController, + AutoConstants.kRobotConfig, + () -> { + Optional alliance = MatchState.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == Alliance.RED; + } + return false; + }, + this + ); + } + } + + @Override + public void periodic() { + estimator.update( + Rotation2d.fromDegrees(getGyroValue()), + new SwerveModulePosition[] { + frontLeft.getPosition(), + frontRight.getPosition(), + rearLeft.getPosition(), + rearRight.getPosition() + } + ); + + frontLeft.periodic(); + frontRight.periodic(); + rearLeft.periodic(); + rearRight.periodic(); + + Logger.recordOutput("Drivetrain/Pose", getPose()); + Logger.recordOutput("Drivetrain/Gyro Angle", getGyroValue()); + Logger.recordOutput("Drivetrain/Heading", getHeadingDegrees()); + Logger.recordOutput("Drivetrain/Velocity", getCurrentChassisSpeeds()); + } + + /** + * Can be used to run an individual module on the drive base a static speed while maintaining a static angle. + * + * Good for diagnosing issues with swerve module configuration. Essentially useless otherwise. + * + * @param name The ModuleName enumeration that indicates which module you want to control + * @param staticSpeed The static speed in Meters Per Second to spin the drive wheel at + * @param staticAngleDegrees The static angle in degrees that you want the wheel to face + * @return A complete Command structure that performs the specified action + */ + public Command runIndividualModule(ModuleName name, double staticSpeed, double staticAngleDegrees) { + SwerveModule module = List.of( + frontLeft, + frontRight, + rearLeft, + rearRight + ).stream() + .filter((m) -> m.getModuleName() == name) + .findFirst() + .get(); + + return run(() -> { + module.setDesiredState(new SwerveModuleVelocity( + staticSpeed, + Rotation2d.fromDegrees(staticAngleDegrees) + )); + }); + } + + public Command disableOutputs() { + return run(() -> { + frontLeft.disableOutput(); + frontRight.disableOutput(); + rearLeft.disableOutput(); + rearRight.disableOutput(); + }); + } + + /** + * Rotates the robot to a face a given Pose2d position on the field + * + * Note that this Command does not provide a means of timeout. If you are + * using this in an auto context, this Command should be decorated with + * withTimeout(). Otherwise, you will be waiting for the PID + * Controller doing the work to report that it is at the desired setpoint. + * + * @param targetPose The Pose2d object to rotate the robot towards + * @param rotate180 When false, the front of the robot faces the specified pose, when true + * the back of the robot faces the specified pose + * @return A complete Command structure that performs the specified action + */ + public Command rotateToPose(Pose2d targetPose, boolean rotate180) { + return lockRotationToSuppliedPose(() -> targetPose, () -> 0, () -> 0, rotate180) + .until(yawRotationController::atSetpoint); + } + + /** + * Locks the robots rotation to face the Alliance Hub on the field. + * + * This method is innately aware of which hub to face based on the assigned alliance color. + * + * This method is NOT for autonomous, see rotateToPose + * + * This method provides a field oriented mechanism of driving the robot, such that the robot + * is always facing the point on the field that is the center of the alliance hub. This + * method assumes that the robots estimated pose is reasonably accurate. + * + * @param xSpeed The X (forward/backward) translational speed of the robot + * @param ySpeed The Y (left/right) translational speed of the robot + * @param rotate180 When false, the front of the robot faces the hub, when true, the back + * of the robot faces the hub + * @return A complete Command structure that performs the specified action + */ + public Command lockRotationToHub(DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) { + return lockRotationToSuppliedPose( + Utilities::getHubPose, + xSpeed, + ySpeed, + rotate180 + ); + } + + /** + * Locks the robots rotation to face a particular pose on the field + * + * This method is NOT for autonomous, see rotateToPose + * + * This method provides a field oriented mechanism of driving the robot, such that the robot + * is always facing the point on the field that is the Pose2d object being supplied. This + * method assumes that the robots estimated pose is reasonably accurate. + * + * @param poseSupplier A Supplier object, lambda, or method reference which consistently produces a Pose2d object to point towards + * @param xSpeed The X (forward/backward) translational speed of the robot + * @param ySpeed The Y (left/right) translational speed of the robot + * @param rotate180 When false, the front of the robot faces the supplied pose, when true, the back + * of the robot faces the supplied pose + * @return A complete Command structure that performs the specified action + */ + public Command lockRotationToSuppliedPose(Supplier poseSupplier, DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) { + return runOnce(yawRotationController::reset).andThen( + drive( + xSpeed, + ySpeed, + () -> { + Pose2d faceTowards = poseSupplier.get(); + + Rotation2d targetRotation = new Rotation2d( + faceTowards.getX() - getPose().getX(), + faceTowards.getY() - getPose().getY() + ); + + if(rotate180) { + targetRotation = targetRotation.rotateBy(Rotation2d.k180deg); + } + + Logger.recordOutput("/HubAutoAlign/CurrentHeader", getHeading().getRadians()); + Logger.recordOutput("/HubAutoAlign/Setpoint", targetRotation.getRadians()); + + double outputPower = -yawRotationController.calculate( + getHeading().getRadians(), + targetRotation.getRadians() + ); + + Logger.recordOutput("/HubAutoAlign/OutputPower", outputPower); + + return outputPower; + }, + () -> true + ) + ); + } + + /** + * A method to lock to a particular source of an external "yaw". The intent is for this yaw to be sourced from + * {@link frc.robot.subsystems.PhotonVision#getBestYawForTag(int)} which generates a "yaw" for a particular tag as referenced + * from the center point of the cameras image frame. The objective being to "0 the source" using a PID Controller, or in + * other terms, to center the provided tag in the camera's image frame. + * + * @param yaw The "yaw" of the tag source relative to the center of the image frame + * @param xSpeed The X (forward/backward) translational speed of the robot + * @param ySpeed The Y (left/right) translational speed of the robot + * @return A complete Command structure that performs the specified action + */ + public Command lockToYaw(DoubleSupplier yaw, DoubleSupplier xSpeed, DoubleSupplier ySpeed) { + return runOnce(yawRotationController::reset).andThen( + drive( + xSpeed, + ySpeed, + () -> yawRotationController.calculate(yaw.getAsDouble(), 0), + () -> true + ) + ); + } + + public Command drivePathPlannerPath(PathPlannerPath path) { + if(AutoConstants.kAutoConfigOk) { + return AutoBuilder.followPath(path); + } else { + return new PrintCommand("Robot Config loading failed, on the fly PathPlanner disabled"); + } + + } + + public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, BooleanSupplier fieldRelative) { + // TODO Specific Alliance code? + return run(() -> { + drive( + -MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband), + -MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband), + -MathUtil.applyDeadband(rotation.getAsDouble(), OIConstants.kDriveDeadband), + fieldRelative.getAsBoolean() + ); + }); + } + + public Command setX() { + return run(() -> { + frontLeft.setDesiredState(new SwerveModuleVelocity(0, Rotation2d.fromDegrees(45))); + frontRight.setDesiredState(new SwerveModuleVelocity(0, Rotation2d.fromDegrees(-45))); + rearLeft.setDesiredState(new SwerveModuleVelocity(0, Rotation2d.fromDegrees(-45))); + rearRight.setDesiredState(new SwerveModuleVelocity(0, Rotation2d.fromDegrees(45))); + }); + } + + public Command zeroHeading() { + return run(() -> { + imu.resetYaw();; + estimator.resetRotation(new Rotation2d(0)); + }); + } + + public void consumeVisualPose(VisualPose pose) { + if(Math.abs(pose.visualPose().minus(getPose()).getTranslation().getNorm()) > 1) { + return; + } + + estimator.addVisionMeasurement( + pose.visualPose(), + pose.timestamp() + ); + } + + public void resetEncoders() { + frontLeft.resetEncoders(); + frontRight.resetEncoders(); + rearLeft.resetEncoders(); + rearRight.resetEncoders(); + } + + public void resetOdometry(Pose2d pose) { + estimator.resetPosition( + Rotation2d.fromDegrees(getGyroValue()), + new SwerveModulePosition[] { + frontLeft.getPosition(), + frontRight.getPosition(), + rearLeft.getPosition(), + rearRight.getPosition() + }, + pose + ); + } + + public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) { + double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)); + double xSpeedDelivered = 0; + double ySpeedDelivered = 0; + + if(p != 0){ + xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond; + ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond; + }else{ + xSpeedDelivered = 0; + ySpeedDelivered = 0; + } + + double rotationDelivered = rotation * DrivetrainConstants.kMaxAngularSpeed; + + ChassisVelocities velocities = new ChassisVelocities(xSpeedDelivered, ySpeedDelivered, rotationDelivered); + + SwerveModuleVelocity[] swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleVelocities( + fieldRelative ? + velocities.toFieldRelative(Rotation2d.fromDegrees(getGyroValue())) : velocities + ); + + setModuleStates(swerveModuleStates); + } + + public void driveWithChassisSpeeds(ChassisVelocities speeds) { + ChassisVelocities discreteSpeeds = speeds.discretize(0.2); + SwerveModuleVelocity[] newStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleVelocities(discreteSpeeds); + + setModuleStates(newStates); + } + + public void setModuleStates(SwerveModuleVelocity[] desiredStates) { + SwerveDriveKinematics.desaturateWheelVelocities( + desiredStates, DrivetrainConstants.kMaxSpeedMetersPerSecond); + frontLeft.setDesiredState(desiredStates[0]); + frontRight.setDesiredState(desiredStates[1]); + rearLeft.setDesiredState(desiredStates[2]); + rearRight.setDesiredState(desiredStates[3]); + } + + public ChassisVelocities getCurrentChassisSpeeds() { + return DrivetrainConstants.kDriveKinematics.toChassisVelocities( + frontLeft.getState(), + frontRight.getState(), + rearLeft.getState(), + rearRight.getState() + ); + } + + public Pose2d getPose() { + return estimator.getEstimatedPosition(); + } + + public double getGyroValue() { + return imu.getYawRadians() * (DrivetrainConstants.kGyroReversed ? -1 : 1); + } + + public Rotation2d getHeading() { + return estimator.getEstimatedPosition().getRotation(); + } + + public double getHeadingDegrees() { + return estimator.getEstimatedPosition().getRotation().getDegrees(); + } +} + diff --git a/src/main/java/first/robot/subsystems/Hood.java b/src/main/java/first/robot/subsystems/Hood.java new file mode 100644 index 0000000..dd61f77 --- /dev/null +++ b/src/main/java/first/robot/subsystems/Hood.java @@ -0,0 +1,183 @@ +package first.robot.subsystems; + +import java.util.Optional; +import java.util.function.DoubleSupplier; + +import org.wpilib.command2.Command; +import org.wpilib.command2.SubsystemBase; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.interpolation.InterpolatingDoubleTreeMap; +import org.wpilib.math.util.MathUtil; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.ControlType; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +import first.robot.constants.HoodConstants; +import first.robot.constants.ShooterConstants.ShooterSpeeds; +import first.robot.utilities.Utilities; + +public class Hood extends SubsystemBase { + private SparkMax motor; + + private RelativeEncoder encoder; + + private SparkClosedLoopController controller; + + //private Trigger resetTrigger; + //private Trigger timerTrigger; + + //private Timer resetTimer; + + private double currentTargetDegrees; + + public Hood() { + motor = new SparkMax( + HoodConstants.kCANPort, + HoodConstants.kMotorCANID, + MotorType.kBrushless + ); + + motor.configure( + HoodConstants.kConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + encoder = motor.getEncoder(); + encoder.setPosition(HoodConstants.kStartupAngle); + + controller = motor.getClosedLoopController(); + + /*resetTimer = new Timer(); + resetTimer.reset(); + + resetTrigger = new Trigger(() -> (motor.getOutputCurrent() > HoodConstants.kAmpsToTriggerPositionReset)); + resetTrigger.onTrue(new InstantCommand(resetTimer::start)); + resetTrigger.onFalse(new InstantCommand(() -> { + resetTimer.stop(); + resetTimer.reset(); + })); + + timerTrigger = new Trigger(() -> resetTimer.hasElapsed(HoodConstants.kTimeAboveThresholdToReset)); + timerTrigger.onTrue(new InstantCommand(() -> { + encoder.setPosition(0); + resetTimer.reset(); + }));*/ + + currentTargetDegrees = HoodConstants.kStartupAngle; + } + + @Override + public void periodic() { + SmartDashboard.putNumber( + "HoodTargetDegrees", + Math.toDegrees(currentTargetDegrees) + ); + + SmartDashboard.putNumber( + "HoodCurrentAngle", + Math.toDegrees(encoder.getPosition()) + ); + + SmartDashboard.putBoolean( + "HoodAtSetpoint", + controller.isAtSetpoint() + ); + + Logger.recordOutput("Hood/OutputCurrent", motor.getOutputCurrent()); + Logger.recordOutput("Hood/CurrentTarget", Math.toDegrees(currentTargetDegrees)); + Logger.recordOutput("Hood/CurrentAngle", Math.toDegrees(encoder.getPosition())); + Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint()); + Logger.recordOutput("Hood/VoltageOut", motor.getAppliedOutput()*motor.getBusVoltage()); + } + + public Command trackToAnglePoseBased(Drivetrain drivetrain, Shooter shooter) { + return trackToAngle(() -> { + Pose2d drivetrainPose = drivetrain.getPose(); + Pose2d hubPose = Utilities.getHubPose(); + + double distance = drivetrainPose.getTranslation() + .getDistance(hubPose.getTranslation()); + + Logger.recordOutput("Hood/DistanceToHub", distance); + + Optional currentSpeeds = shooter.getTargetSpeeds(); + + if(currentSpeeds.isPresent()) { + InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get()); + + if(map != null) { + return MathUtil.clamp(map.get(distance), 0, 40); + } else { + return 0; + } + } else { + return 0; + } + }); + } + + public Command trackToAngle(DoubleSupplier degreeAngleSupplier) { + return run(() -> { + currentTargetDegrees = degreeAngleSupplier.getAsDouble(); + + controller.setSetpoint(currentTargetDegrees, ControlType.kPosition); + }); + } + + /** + * An automated form of resetting the hood position sensing. + * + * Run down at the full manual speed (note that this is affected by the + * kMaxManualSpeedMultiplier constant) until the timer trigger becomes true + * (i.e. the output current has been above the threshold (kAmpsToTriggerPositionReset) + * for reset for the amount of specified by kTimeAboveThresholdToReset). Once + * that returns true, the motor is stopped until the timer trigger switches to false + * (i.e. it has reset the position automatically, because that's how it's configured, + * and resets the timer to 0, which makes the timer trigger false) + * + * @return A complete Command structure that performs the specified action + */ + /*public Command automatedRezero() { + return manualSpeed(() -> -1) + .until(timerTrigger) + .andThen( + stop().until(timerTrigger.negate()) + ); + } + + /** + * An alternate form of {@link #automatedRezero()} that doesn't rely on the triggers + * to reset the hood position to zero. Note that this method doesn't have any time limiting + * factor to it, as soon as the current goes above the threshold specified by + * kAmpsToTriggerPositionReset the encoder position will be set to zero + * + * @return A complete Command structure that performs the specified action + */ + /*public Command automatedRezeroNoTimer() { + return manualSpeed(() -> -1) + .until(() -> motor.getOutputCurrent() >= HoodConstants.kAmpsToTriggerPositionReset) + .andThen(new InstantCommand(() -> encoder.setPosition(0))); + }*/ + + public Command manualSpeed(DoubleSupplier speed) { + currentTargetDegrees = 0; + + return run(() -> { + motor.setThrottle(speed.getAsDouble() * HoodConstants.kMaxManualSpeedMultiplier); + }); + } + + public Command stop() { + return manualSpeed(() -> 0); + } + + public double getTargetDegrees() { + return currentTargetDegrees; + } +} diff --git a/src/main/java/first/robot/subsystems/IntakePivot.java b/src/main/java/first/robot/subsystems/IntakePivot.java new file mode 100644 index 0000000..e8699b8 --- /dev/null +++ b/src/main/java/first/robot/subsystems/IntakePivot.java @@ -0,0 +1,125 @@ +package first.robot.subsystems; + +import java.util.Optional; +import java.util.function.DoubleSupplier; + +import org.wpilib.command2.Command; +import org.wpilib.command2.Commands; +import org.wpilib.command2.SubsystemBase; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.ControlType; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +import first.robot.constants.IntakePivotConstants; +import first.robot.constants.IntakePivotConstants.IntakePivotPosition; + +public class IntakePivot extends SubsystemBase { + private SparkMax leftMotor; + private SparkMax rightMotor; + + private RelativeEncoder encoder; + + private SparkClosedLoopController controller; + + private IntakePivotPosition currentTargetPosition; + + public IntakePivot() { + leftMotor = new SparkMax( + IntakePivotConstants.kCANPort, + IntakePivotConstants.kLeftMotorCANID, + MotorType.kBrushless + ); + rightMotor = new SparkMax( + IntakePivotConstants.kCANPort, + IntakePivotConstants.kRightMotorCANID, + MotorType.kBrushless + ); + + leftMotor.configure( + IntakePivotConstants.KLeftMotorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + rightMotor.configure( + IntakePivotConstants.kRightMotorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + controller = leftMotor.getClosedLoopController(); + + encoder = leftMotor.getEncoder(); + encoder.setPosition(IntakePivotConstants.IntakePivotPosition.kUp.getPositionRadians()); + } + + @Override + public void periodic() { + Logger.recordOutput( + "IntakePivot/TargetPosition", + currentTargetPosition == null ? -1 : currentTargetPosition.getPositionRadians()); + Logger.recordOutput("IntakePivot/CurrentPosition", encoder.getPosition()); + Logger.recordOutput("IntakePivot/AtSetpoint", controller.isAtSetpoint()); + } + + public Command maintainPosition(IntakePivotPosition position) { + return run(() -> { + currentTargetPosition = position; + + if(currentTargetPosition == null) { + leftMotor.disable(); + } else { + controller.setSetpoint(currentTargetPosition.getPositionRadians(), ControlType.kPosition); + } + }); + } + + public Command manualSpeed(DoubleSupplier speed) { + return run(() -> { + currentTargetPosition = null; + + leftMotor.setThrottle(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier); + rightMotor.setThrottle(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier); + }); + } + + /** + * Repeatedly moves the intake up and down. AKA "Jimmying" the intake + * + * @param time How long the intake will go both ways for (seconds) + * @return Command that repeatedly Jimmys the intake + */ + public Command jimmy(double time){ + return Commands.repeatingSequence( + manualSpeed(() -> -0.75).withTimeout(time), + manualSpeed(() -> 0.75).withTimeout(time) + ); + } + + /** + * Repeatedly moves the intake up and down. AKA "Jimmying" the intake + * + * @param time How long the intake will go both ways for (seconds) + * @return Command that repeatedly Jimmys the intake + */ + public Command jimmy(DoubleSupplier time) { + return Commands.repeatingSequence( + manualSpeed(() -> -0.75).withTimeout(time.getAsDouble()), + manualSpeed(() -> 0.75).withTimeout(time.getAsDouble()) + ); + } + + public Command stop() { + return manualSpeed(() -> 0); + } + + public Optional getCurrentTargetPosition() { + return Optional.ofNullable(currentTargetPosition); + } +} + diff --git a/src/main/java/first/robot/subsystems/IntakeRoller.java b/src/main/java/first/robot/subsystems/IntakeRoller.java new file mode 100644 index 0000000..c46f24c --- /dev/null +++ b/src/main/java/first/robot/subsystems/IntakeRoller.java @@ -0,0 +1,64 @@ +package first.robot.subsystems; + +import org.wpilib.command2.Command; +import org.wpilib.command2.SubsystemBase; + +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +import first.robot.constants.IntakeRollerConstants; + +public class IntakeRoller extends SubsystemBase { + private SparkMax leftMotor; + private SparkMax rightMotor; + + public IntakeRoller() { + leftMotor = new SparkMax( + IntakeRollerConstants.kCANPort, + IntakeRollerConstants.kLeftMotorCANID, + MotorType.kBrushless + ); + + rightMotor = new SparkMax( + IntakeRollerConstants.kCANPort, + IntakeRollerConstants.kRightMotorCANID, + MotorType.kBrushless + ); + + leftMotor.configure( + IntakeRollerConstants.leftMotorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + rightMotor.configure( + IntakeRollerConstants.rightMotorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + } + + public Command runIn() { + return run(() -> { + leftMotor.setThrottle(IntakeRollerConstants.kIntakeInSpeed); + rightMotor.setThrottle(IntakeRollerConstants.kIntakeInSpeed); + }); + } + + public Command runOut() { + return run(() -> { + leftMotor.setThrottle(-IntakeRollerConstants.kIntakeOutSpeed); + rightMotor.setThrottle(-IntakeRollerConstants.kIntakeOutSpeed); + }); + } + + public Command stop() { + return run(() -> { + leftMotor.setThrottle(0); + rightMotor.setThrottle(0); + }); + } + +} diff --git a/src/main/java/first/robot/subsystems/PhotonVision.java b/src/main/java/first/robot/subsystems/PhotonVision.java new file mode 100644 index 0000000..c81bfc7 --- /dev/null +++ b/src/main/java/first/robot/subsystems/PhotonVision.java @@ -0,0 +1,204 @@ +package first.robot.subsystems; + +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import java.util.OptionalDouble; +import java.util.function.Consumer; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.wpilib.command2.SubsystemBase; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Transform3d; + +import first.robot.constants.CompetitionConstants; +import first.robot.constants.PhotonConstants; +import first.robot.utilities.PhotonVisionConfig; +import first.robot.utilities.VisualPose; + +/** + * This "Subsystem" is not actually a Subsystem. The intent is for this to be treated as + * a "resource", that is, something that is not inherently a physical mechanism to be controlled. + * + * A "resource" in this instance should be thought of as something that can be safely shared + * by other Subsystems generally without collision if more that one Subsystem requires the + * "resource" at any given time. + * + * Resources should NOT produce Commands, they should not have a default Command. + * Resources do not have behaviors, and because Commands are in of themselves behaviors, + * this class should not have Commands. + * + * Part of the thinking behind creating the PhotonVision components this way is to rely + * on the CommandScheduler to call periodic. If this weren't the case, some other subsystem + * would have to manage calling for periodic updates, while still sharing the resource with + * other subsystems somehow. + * + * This class is dynamic, by adding or removing PhotonVisionConfig objects to the "configs" + * List in the PhotonConstants file, you change what is set up internally in this class. + * 1 config means 1 camera, 1 estimator, 1 stored pipeline result, 2 configs means 2 cameras, + * 2 estimators, etc. etc. + */ +public class PhotonVision extends SubsystemBase { + private PhotonCamera[] cameras; + private PhotonPoseEstimator[] estimators; + private List latestResults; + + private ArrayList> poseEstimateConsumers; + + public PhotonVision() { + cameras = new PhotonCamera[PhotonConstants.configs.size()]; + estimators = new PhotonPoseEstimator[PhotonConstants.configs.size()]; + latestResults = new ArrayList(); + + for(int i = 0; i < PhotonConstants.configs.size(); i++) { + cameras[i] = new PhotonCamera(PhotonConstants.configs.get(i).cameraName()); + estimators[i] = new PhotonPoseEstimator( + CompetitionConstants.kField, + PhotonConstants.configs.get(i).robotToCamera() + ); + latestResults.add(null); + } + + poseEstimateConsumers = new ArrayList>(); + } + + @Override + public void periodic() { + for(int i = 0; i < cameras.length; i++) { + List results = cameras[i].getAllUnreadResults(); + + if(!results.isEmpty()) { + latestResults.set(i, results.get(results.size() - 1)); + + Optional pose = estimators[i].estimateCoprocMultiTagPose(latestResults.get(i)); + + if(!pose.isEmpty()) { + VisualPose visualPose = new VisualPose( + cameras[i].getName(), + pose.get().estimatedPose.toPose2d(), + pose.get().timestampSeconds + ); + + for(Consumer consumer: poseEstimateConsumers) { + consumer.accept(visualPose); + } + } + } + } + } + + /** + * Returns the best 3D pose for a given AprilTag ID as seen by the cameras on the robot. + * + * All cameras fields of view are observed, if no camera can see the given tag ID, this + * method will return Optional.empty(). + * + * Note that this method has no minimum confidence threshold for a tag. This means that + * if one camera thinks it sees the tag, even with very low confidence, it'll still return + * some sort of pose. + * + * @param tagID The ID of the tag to look for in the latest results from all cameras + * @return An Optional object containing a Pose3d object, or Optional.empty() if + * the tag is not present anywhere in the robots field of view. + */ + public Optional getBestPoseForTag(int tagID) { + PhotonVisionConfig config = null; + Transform3d bestCameraToTarget = null; + float bestConfidence = -1; + + for(int cameraIndex = 0; cameraIndex < latestResults.size(); cameraIndex++) { + if(latestResults.get(cameraIndex) == null) { + continue; + } + + for(PhotonTrackedTarget target: latestResults.get(cameraIndex).getTargets()) { + if(target.getFiducialId() != tagID || bestConfidence > target.getDetectedObjectConfidence()) { + continue; + } + + config = PhotonConstants.configs.get(cameraIndex); + bestCameraToTarget = target.bestCameraToTarget; + bestConfidence = target.getDetectedObjectConfidence(); + } + } + + if(bestCameraToTarget == null) { + return Optional.empty(); + } + + // This is based on what PhotonVision does for multitag Pose estimation + // See PhotonPoseEstimator.multiTagOnCoprocStrategy + // TODO This doesn't currently account for the offset of the tag relative to say the hub + // unclear if that offset amount will be important or not + return Optional.of(Pose3d.ZERO + .plus(bestCameraToTarget.inverse()) + .relativeTo(CompetitionConstants.kField.getOrigin()) + .plus(config.robotToCamera().inverse())); + } + + /** + * Returns the best yaw for a given AprilTag ID as seen by the cameras on the robot. + * + * All cameras fields of view are observed, if no camera can see the given tag ID, this + * method will return OptionalDouble.empty() + * + * Note that this method has no minimum confidence threshold for a tag. This means that + * if one camera thinks it sees the tag, even with very low confidence, it'll still + * return some sort of yaw value. + * + * Note that the yaw value here is the yaw of the observed tag relative to the center + * of the cameras image frame. + * + * @param tagID The ID of the tag to look for in the latest results from all cameras + * @return An OptionalDouble object containing a Double representing the described yaw + * of the AprilTag specified, or OptionalDouble.empty() if the tag is not present + * anywhere in the robots field of view + */ + public OptionalDouble getBestYawForTag(int tagID) { + double bestTagYaw = -1; + float bestConfidence = -1; + + for(PhotonPipelineResult result: latestResults) { + if(result == null) { + continue; + } + + for(PhotonTrackedTarget target: result.getTargets()) { + if(target.getFiducialId() != tagID || bestConfidence > target.getDetectedObjectClassID()) { + continue; + } + + bestTagYaw = target.getYaw(); + bestConfidence = target.getDetectedObjectConfidence(); + } + } + + if(bestConfidence == -1) { + return OptionalDouble.empty(); + } + + return OptionalDouble.of(bestTagYaw); + } + + /** + * Add a Consumer of VisualPose records to the PhotonVision resource. + * + * Each consumer will receive a VisualPose object when any camera produces a new + * VisualPose. + * + * The number of Poses produced in a given 20ms cycle is the same number as how many + * cameras there are on the robot, assuming those cameras see enough tags to generate a pose, + * as currently all cameras configuration will generate a Pose2d + * + * @param consumer The lambda, functional reference, or Consumer implementing object + * that will consume Poses produced by the PhotonVision resource. + */ + public void addPoseEstimateConsumer(Consumer consumer) { + poseEstimateConsumers.add(consumer); + } +} + diff --git a/vendordeps/photonlib-v2027.0.0-alpha-2.json b/vendordeps/photonlib-v2027.0.0-alpha-2.json index 1cb9ff7..6d0670f 100644 --- a/vendordeps/photonlib-v2027.0.0-alpha-2.json +++ b/vendordeps/photonlib-v2027.0.0-alpha-2.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2027.0.0-alpha-2", + "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "wpilibYear": "2027_alpha7", "mavenUrls": [ @@ -13,7 +13,20 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2027.0.0-alpha-2", + "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxsystemcore", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.wpilib.fields", + "artifactId": "fields-cpp", + "version": "wpilib", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +41,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2027.0.0-alpha-2", + "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +56,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2027.0.0-alpha-2", + "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +73,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2027.0.0-alpha-2" + "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2027.0.0-alpha-2" + "version": "dev-v2027.0.0-alpha-2-81-g5b5b7870" } ] }