diff --git a/build.gradle b/build.gradle index d141f06..5159c6c 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.3.1" } java { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 93f477c..c8ae460 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,104 +25,80 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - /** - * Input/Output constants - */ - public static final class IOConstants { - public static final int kDriverControllerPort = 0; - - public static final double kControllerDeadband = 0.2; - public static final double kSlowModeScalar = 0.8; - } - - public static final class DriveConstants { - // TODO: set motor and encoder constants - public static final int kFrontLeftDriveMotorPort = 32; - public static final int kRearLeftDriveMotorPort = 29; - public static final int kFrontRightDriveMotorPort = 38; - public static final int kRearRightDriveMotorPort = 34; - - public static final int kFrontLeftTurningMotorPort = 28; - public static final int kRearLeftTurningMotorPort = 22; - public static final int kFrontRightTurningMotorPort = 37; - public static final int kRearRightTurningMotorPort = 26; - - public static final int kFrontLeftTurningEncoderPort = 19; - public static final int kRearLeftTurningEncoderPort = 20; - public static final int kFrontRightTurningEncoderPort = 18; - public static final int kRearRightTurningEncoderPort = 17; - - public static final double kFrontLeftTurningEncoderOffset = 0; - public static final double kRearLeftTurningEncoderOffset = 0; - public static final double kFrontRightTurningEncoderOffset = 0; - public static final double kRearRightTurningEncoderOffset = 0; - - // TODO: Test motor orientations before driving on an actual robot - public static final boolean kFrontLeftDriveMotorReversed = false; - public static final boolean kRearLeftDriveMotorReversed = false; - public static final boolean kFrontRightDriveMotorReversed = true; - public static final boolean kRearRightDriveMotorReversed = true; - - /** Distance between centers of right and left wheels on robot (in meters). */ - public static final double kTrackWidth = 0.57785; - - /** Distance between front and back wheels on robot (in meters). */ - public static final double kWheelBase = 0.57785; - - /** Diameter of each wheel in the SDS MK4i swerve module (in meters) */ - public static final double kWheelDiameterMeters = 0.1; - - /** Gear ratio between the motor and the wheel. */ - public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration - // public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 - // configuration - - // TODO: Tune this PID before running on a robot on the ground - public static final double kPModuleTurningController = -0.3; - - public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2, kTrackWidth / 2), - new Translation2d(kWheelBase / 2, -kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); - - /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ - public static final double kMaxSpeedMetersPerSecond = 4.4196; - /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ - public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164; - // ^^ Calculated using the method taken from the old SDS github example - - /** Heading Correction */ - public static final double kHeadingCorrectionTurningStopTime = 0.2; - // TODO: Tune this PID before running on a robot on the ground - public static final double kPHeadingCorrectionController = 5; - } - - // Intake PID and Encoder Constants - public static class IntakeConstants { - public static final double kIntakeLoweredAngle = 9.0; - public static final double kIntakeRaisedAngle = 9.0; - public static final int kIntakeMotorID = 0; - public static final int kArmMotorID = 0; - public static final double kIntakeP = 0; - public static final double kIntakeI = 0; - public static final double kIntakeD = 0; - public static final double kArmP = 0; - public static final double kArmI = 0; - public static final double kArmD = 0; - public static final int kArmEncoderCh = 0; - public static double kIntakeSpeed; - } - - // Shooter subsystem speed constants - public static class ShooterConstants { - public static final double kSpinSpeedTrue = 0.75; - public static final double kSpinSpeedFalse = 0; - public static int kBottomShooterMotorPort; - public static int kTopShooterMotorPort; - } - public static class ClimberConstants { + /** + * Input/Output constants + */ + public static final class IOConstants { + public static final int kDriverControllerPort = 0; + public static final int kOperatorControllerPort = 1; + + public static final double kControllerDeadband = 0.05; + public static final double kSlowModeScalar = 0.8; + } + + public static final class DriveConstants { + // TODO: set motor and encoder constants + public static final int kFrontLeftDriveMotorPort = 32; + public static final int kRearLeftDriveMotorPort = 29; + public static final int kFrontRightDriveMotorPort = 38; + public static final int kRearRightDriveMotorPort = 34; + + public static final int kFrontLeftTurningMotorPort = 28; + public static final int kRearLeftTurningMotorPort = 22; + public static final int kFrontRightTurningMotorPort = 37; + public static final int kRearRightTurningMotorPort = 26; + + public static final int kFrontLeftTurningEncoderPort = 5; + public static final int kRearLeftTurningEncoderPort = 6; + public static final int kFrontRightTurningEncoderPort = 3; + public static final int kRearRightTurningEncoderPort = 4; + + // TODO: Test motor orientations before driving on an actual robot + public static final boolean kFrontLeftDriveMotorReversed = false; + public static final boolean kRearLeftDriveMotorReversed = false; + public static final boolean kFrontRightDriveMotorReversed = true; + public static final boolean kRearRightDriveMotorReversed = true; + + /** Distance between centers of right and left wheels on robot (in meters). */ + public static final double kTrackWidth = 0.57785; + + /** Distance between front and back wheels on robot (in meters). */ + public static final double kWheelBase = 0.57785; + + /** Diameter of each wheel in the SDS MK4i swerve module (in meters) */ + public static final double kWheelDiameterMeters = 0.1; + + /** Gear ratio between the motor and the wheel. */ + public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration + // public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 configuration + + // TODO: Tune this PID before running on a robot on the ground + public static final double kPModuleTurningController = 0.3; + + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + + /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ + public static final double kMaxSpeedMetersPerSecond = 4.4196; + /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ + public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164; + // ^^ Calculated using the method taken from the old SDS github example + + /** Heading Correction */ + public static final double kHeadingCorrectionTurningStopTime = 0.2; + // TODO: Tune this PID before running on a robot on the ground + public static final double kPHeadingCorrectionController = 5; + } + + public static final class ShooterConstants { + public static final int kTopShooterMotorPort = 35; + public static final int kBottomShooterMotorPort = 20; + } + public static class ClimberConstants { public final static int leftForwardChannel = 0; public final static int rightForwardChannel = 0; public final static int leftReverseChannel = 1; @@ -133,24 +109,24 @@ public static class ClimberConstants { } + public static final class VisionConstants { + // TODO: Update cam pose relative to center of bot + public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); + public static final double[] kLimelightCamPose = { + kCamPose.getX(), + kCamPose.getY(), + kCamPose.getZ(), + kCamPose.getRotation().getX(), + kCamPose.getRotation().getY(), + kCamPose.getRotation().getZ() }; + + // TODO: Experiment with different std devs in the pose estimator + public static final Vector kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.01); + public static final Vector kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9); + + // Field size in meters + public static final double kFieldWidth = 8.21055; + public static final double kFieldLength = 16.54175; + } - public static final class VisionConstants { - // TODO: Update cam pose relative to center of bot - public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); - public static final double[] kLimelightCamPose = { - kCamPose.getX(), - kCamPose.getY(), - kCamPose.getZ(), - kCamPose.getRotation().getX(), - kCamPose.getRotation().getY(), - kCamPose.getRotation().getZ() }; - - // TODO: Experiment with different std devs in the pose estimator - public static final Vector kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1); - public static final Vector kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9); - - // Field size in meters - public static final double kFieldWidth = 8.21055; - public static final double kFieldLength = 16.54175; - } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ce342f7..659a473 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,11 +4,20 @@ package frc.robot; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; + import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -27,16 +36,28 @@ public class RobotContainer { // The robot's subsystems and commands are defined here private final DriveSubsystem m_robotDrive = new DriveSubsystem(); - public final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); + private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); - private final XboxController m_operatorController = new XboxController(IOConstants.kDriverControllerPort); + private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort); /** - * The container for the robot. Contains subsystems, OI devices, and commands. + * The container for the robot. Contains subsystems, IO devices, and commands. */ public RobotContainer() { + + AutoBuilder.configureHolonomic(m_robotDrive::getPose, m_robotDrive::resetOdometry, + m_robotDrive::getChassisSpeeds, + m_robotDrive::autonDrive, + new HolonomicPathFollowerConfig( + new PIDConstants(5, 0.0, 0.0), // Translation PID constants + new PIDConstants(5, 0.0, 0.0), // Rotation PID constants + DriveConstants.kMaxSpeedMetersPerSecond, // Max module speed, in m/s + Math.hypot(DriveConstants.kTrackWidth, DriveConstants.kWheelBase), // Drive base radius in meters. Distance from robot center to furthest module. + new ReplanningConfig(true, true)), + () -> false, m_robotDrive); + // Configure the trigger bindings configureBindings(); @@ -49,18 +70,18 @@ public RobotContainer() { * DriveConstants.kMaxSpeedMetersPerSecond * (1 - m_driverController .getLeftTriggerAxis() - * IOConstants.kSlowModeScalar) - * 0.8, + * IOConstants.kSlowModeScalar), + // * 0.8, MathUtil.applyDeadband( -m_driverController.getLeftX(), IOConstants.kControllerDeadband) * DriveConstants.kMaxSpeedMetersPerSecond * (1 - m_driverController .getLeftTriggerAxis() - * IOConstants.kSlowModeScalar) - * 0.8, + * IOConstants.kSlowModeScalar), + // * 0.8, MathUtil.applyDeadband( - m_driverController.getRightX(), + -m_driverController.getRightX(), IOConstants.kControllerDeadband) * DriveConstants.kMaxAngularSpeedRadiansPerSecond * (1 - m_driverController @@ -78,17 +99,16 @@ private void configureBindings() { new JoystickButton(m_driverController, Button.kStart.value) .onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive)); - // TODO: Move shoot commands to operator controller - new JoystickButton(m_driverController, Button.kB.value) + // new JoystickButton(m_driverController, Button.kA.value).whileTrue( + // AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new PathConstraints( + // DriveConstants.kMaxSpeedMetersPerSecond - 1, 5, DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5))); + + new JoystickButton(m_operatorController, Button.kX.value) .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem)) .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); - new JoystickButton(m_driverController, Button.kA.value) + new JoystickButton(m_operatorController, Button.kY.value) .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem)) .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); - new JoystickButton(m_operatorController, Button.kA.value) - .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_shooterSubsystem)); - new JoystickButton(m_operatorController, Button.kB.value) - .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_shooterSubsystem)); } /** @@ -97,6 +117,20 @@ private void configureBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + PathPlannerPath path = PathPlannerPath.fromPathFile("New New Path"); + + var alliance = DriverStation.getAlliance(); + PathPlannerPath autonPath = path; + if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { + autonPath = autonPath.flipPath(); + } + m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose()); + + // return AutoBuilder.followPath(autonPath); + return null; + + // PathPlannerAuto pathPlannerAuto = new PathPlannerAuto("New Auto"); + + // return pathPlannerAuto; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index f606d5b..6df1fae 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -4,6 +4,8 @@ package frc.robot.subsystems; +import java.util.Optional; + import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.math.MathUtil; @@ -22,35 +24,32 @@ import frc.robot.Constants.DriveConstants; import frc.robot.Constants.VisionConstants; import frc.robot.Robot; +import frc.robot.subsystems.VisionSubsystem.Measurement; public class DriveSubsystem extends SubsystemBase { private final SwerveModule m_frontLeft = new SwerveModule( DriveConstants.kFrontLeftDriveMotorPort, DriveConstants.kFrontLeftTurningMotorPort, DriveConstants.kFrontLeftTurningEncoderPort, - DriveConstants.kFrontLeftDriveMotorReversed, - DriveConstants.kFrontLeftTurningEncoderOffset); + DriveConstants.kFrontLeftDriveMotorReversed); private final SwerveModule m_rearLeft = new SwerveModule( DriveConstants.kRearLeftDriveMotorPort, DriveConstants.kRearLeftTurningMotorPort, DriveConstants.kRearLeftTurningEncoderPort, - DriveConstants.kRearLeftDriveMotorReversed, - DriveConstants.kRearLeftTurningEncoderOffset); + DriveConstants.kRearLeftDriveMotorReversed); private final SwerveModule m_frontRight = new SwerveModule( DriveConstants.kFrontRightDriveMotorPort, DriveConstants.kFrontRightTurningMotorPort, DriveConstants.kFrontRightTurningEncoderPort, - DriveConstants.kFrontRightDriveMotorReversed, - DriveConstants.kFrontRightTurningEncoderOffset); + DriveConstants.kFrontRightDriveMotorReversed); private final SwerveModule m_rearRight = new SwerveModule( DriveConstants.kRearRightDriveMotorPort, DriveConstants.kRearRightTurningMotorPort, DriveConstants.kRearRightTurningEncoderPort, - DriveConstants.kRearRightDriveMotorReversed, - DriveConstants.kRearRightTurningEncoderOffset); + DriveConstants.kRearRightDriveMotorReversed); private final AHRS m_gyro = new AHRS(); private double m_gyroAngle; @@ -65,10 +64,15 @@ public class DriveSubsystem extends SubsystemBase { m_rearRight.getPosition() }; + SwerveModuleState[] swerveModuleStates = { new SwerveModuleState(), new SwerveModuleState(), new SwerveModuleState(), + new SwerveModuleState() }; + private final SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator(DriveConstants.kDriveKinematics, m_gyro.getRotation2d(), m_swerveModulePositions, new Pose2d(), VisionConstants.kOdometrySTDDevs, VisionConstants.kVisionSTDDevs); + private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); + private final Field2d m_field = new Field2d(); /** Creates a new DriveSubsystem. */ @@ -92,6 +96,14 @@ public void periodic() { m_poseEstimator.update(Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle), m_swerveModulePositions); + Optional latestReading = m_visionSubsystem.getMeasurement(); + + // SmartDashboard.putBoolean("reading present", latestReading.isPresent()); + + if (latestReading.isPresent()) { + m_poseEstimator.addVisionMeasurement(latestReading.get().pose.toPose2d(), latestReading.get().timestamp); + } + m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); SmartDashboard.putNumber("gyro angle", m_gyro.getAngle()); @@ -106,6 +118,8 @@ public void periodic() { m_rearRight.getPosition().angle.getDegrees(), m_rearRight.driveOutput, }; SmartDashboard.putNumberArray("AdvantageScope Swerve States", logData); + + setModuleStates(swerveModuleStates); } /** @@ -165,13 +179,15 @@ public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRe // Depending on whether the robot is being driven in field relative, calculate // the desired states for each of the modules - SwerveModuleState[] swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( + swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, calculatedRotation, Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle)) : new ChassisSpeeds(xSpeed, ySpeed, calculatedRotation)); + } - setModuleStates(swerveModuleStates); + public ChassisSpeeds getChassisSpeeds() { + return DriveConstants.kDriveKinematics.toChassisSpeeds(swerveModuleStates); } /** @@ -202,13 +218,13 @@ public void addVisionMeasurement(Pose2d pose, double timestamp) { } /** - * Sets the swerve ModuleStates. + * Sets the swerve ModuleStates. Overloaded for either auton builder or teleop. * * @param desiredStates The desired SwerveModule states. */ public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds( - desiredStates, DriveConstants.kMaxSpeedMetersPerSecond); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, DriveConstants.kMaxSpeedMetersPerSecond); + m_frontLeft.setDesiredState(desiredStates[0]); m_frontRight.setDesiredState(desiredStates[1]); m_rearLeft.setDesiredState(desiredStates[2]); @@ -229,4 +245,8 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { * Robot.kDefaultPeriod; } + public void autonDrive(ChassisSpeeds desiredChassisSpeeds) { + swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(desiredChassisSpeeds); + } + } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index a47de12..afdbd19 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -29,4 +29,4 @@ public void periodic() { // This method will be called once per scheduler run // SmartDashboard.putNumber("Speed", m_bottom.); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 9d98ea8..674e125 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -4,12 +4,10 @@ package frc.robot.subsystems; -import com.ctre.phoenix6.configs.CANcoderConfigurator; -import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; @@ -19,11 +17,10 @@ import frc.robot.Robot; public class SwerveModule { - private final CANSparkMax m_driveMotor; - private final CANSparkMax m_turningMotor; + private final CANSparkFlex m_driveMotor; + private final CANSparkFlex m_turningMotor; private final CANcoder m_turningEncoder; - private final CANcoderConfigurator m_turningEncoderConfigurator; private final PIDController m_turningPIDController = new PIDController(DriveConstants.kPModuleTurningController, 0, 0); @@ -37,34 +34,33 @@ public class SwerveModule { /** * Constructs a {@link SwerveModule}. * - * @param driveMotorPort The port of the drive motor. - * @param turningMotorPort The port of the turning motor. - * @param turningEncoderPort The port of the turning encoder. - * @param driveMotorReversed Whether the drive motor is reversed. - * @param turningEncoderOffset Offset of the turning encoder. + * @param driveMotorPort The port of the drive motor. + * @param turningMotorPort The port of the turning motor. + * @param turningEncoderPort The port of the turning encoder. + * @param driveMotorReversed Whether the drive motor is reversed. */ public SwerveModule( int driveMotorPort, int turningMotorPort, int turningEncoderPort, - boolean driveMotorReversed, - double turningEncoderOffset) { - m_driveMotor = new CANSparkMax(driveMotorPort, MotorType.kBrushless); - m_turningMotor = new CANSparkMax(turningMotorPort, MotorType.kBrushless); + boolean driveMotorReversed) { + m_driveMotor = new CANSparkFlex(driveMotorPort, MotorType.kBrushless); + m_turningMotor = new CANSparkFlex(turningMotorPort, MotorType.kBrushless); m_turningEncoder = new CANcoder(turningEncoderPort); - m_turningEncoderConfigurator = m_turningEncoder.getConfigurator(); // converts default units to meters per second m_driveMotor.getEncoder().setVelocityConversionFactor( DriveConstants.kWheelDiameterMeters * Math.PI / 60 / DriveConstants.kDrivingGearRatio); + m_driveMotor.getEncoder().setPositionConversionFactor( + DriveConstants.kWheelDiameterMeters * Math.PI / DriveConstants.kDrivingGearRatio); m_driveMotor.setInverted(driveMotorReversed); - m_turningMotor.setIdleMode(IdleMode.kBrake); + m_driveMotor.setIdleMode(IdleMode.kBrake); + + m_turningMotor.setInverted(true); - // TODO: CANcoder offsets are now set on the device manually using Pheonix Tuner - // (or maybe Pheonix X) - m_turningEncoderConfigurator.apply(new MagnetSensorConfigs().withMagnetOffset(-turningEncoderOffset)); + m_turningMotor.setIdleMode(IdleMode.kBrake); m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI); } @@ -83,7 +79,7 @@ public SwerveModulePosition getPosition() { // expected values return Robot.isReal() ? new SwerveModulePosition(m_driveMotor.getEncoder().getPosition(), - getEncoderAngle(m_turningEncoder)) + getTurnEncoderAngle()) : new SwerveModulePosition(m_distance, m_state.angle); } @@ -93,10 +89,10 @@ public SwerveModulePosition getPosition() { * @param desiredState Desired state with speed and angle. */ public void setDesiredState(SwerveModuleState desiredState) { - m_state = SwerveModuleState.optimize(desiredState, getEncoderAngle(m_turningEncoder)); + m_state = SwerveModuleState.optimize(desiredState, getTurnEncoderAngle()); driveOutput = m_state.speedMetersPerSecond / DriveConstants.kMaxSpeedMetersPerSecond; - turnOutput = m_turningPIDController.calculate(getEncoderAngle(m_turningEncoder).getRadians(), + turnOutput = m_turningPIDController.calculate(getTurnEncoderAngle().getRadians(), m_state.angle.getRadians()); m_driveMotor.set(driveOutput); @@ -104,15 +100,14 @@ public void setDesiredState(SwerveModuleState desiredState) { } /** - * Returns the angle of a CANcoder + * Returns the angle of the turning CANcoder * * The CANcoder now gives values in rotations which is useless, so this method * translates the CANcoder output into a Rotation2D * - * @param encoder The encoder to get the absolute angle of. * @return A Rotation2d of the absolute angle. */ - public static Rotation2d getEncoderAngle(CANcoder encoder) { - return new Rotation2d(encoder.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI); + public Rotation2d getTurnEncoderAngle() { + return new Rotation2d(m_turningEncoder.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI); } } diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index daeea25..e22d3e0 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -15,10 +15,12 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.DoubleArraySubscriber; -import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.IntegerSubscriber; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.TimestampedDoubleArray; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.VisionConstants; @@ -50,8 +52,8 @@ public class VisionSubsystem extends SubsystemBase { NetworkTable m_visionNetworkTable = NetworkTableInstance.getDefault().getTable("limelight"); private final DoubleArraySubscriber m_botPose; - private final DoubleSubscriber m_cl; - private final DoubleSubscriber m_tl; + + private final IntegerSubscriber m_tv; /** Creates a new Limelight. */ public VisionSubsystem() { @@ -61,13 +63,14 @@ public VisionSubsystem() { // Create subscribers to get values from the limelight m_botPose = m_visionNetworkTable.getDoubleArrayTopic("botpose_wpiblue").subscribe(null); - m_cl = m_visionNetworkTable.getDoubleTopic("cl").subscribe(0); - m_tl = m_visionNetworkTable.getDoubleTopic("tl").subscribe(0); + m_tv = m_visionNetworkTable.getIntegerTopic("tv").subscribe(0); } @Override public void periodic() { // This method will be called once per scheduler run + + SmartDashboard.putBoolean("Limelight Has Target", m_tv.get() == 1); } public Optional getMeasurement() { @@ -81,8 +84,9 @@ public Optional getMeasurement() { TimestampedDoubleArray update = updates[updates.length - 1]; - // If the latest update is empty then return nothing - if (Arrays.equals(update.value, new double[6])) { + // If the latest update is empty or we don't see an april tag then return + // nothing + if (Arrays.equals(update.value, new double[6]) || m_tv.get() == 0) { return Optional.empty(); } @@ -93,21 +97,9 @@ public Optional getMeasurement() { double pitch = Units.degreesToRadians(update.value[4]); double yaw = Units.degreesToRadians(update.value[5]); - double latency = m_cl.get() + m_tl.get(); - - double timestamp = (update.timestamp * 1e-6) - (latency * 1e-3); + double timestamp = Timer.getFPGATimestamp() - (update.value[6] / 1000.0); Pose3d pose = new Pose3d(new Translation3d(x, y, z), new Rotation3d(roll, pitch, yaw)); - /* - * The limelight returns 3D field poses where (0, 0, 0) is located at the center - * of the field - * - * So to input this pose into our pose estimator we need to tranform so that (0, - * 0, 0) is the right corner of the blue driver stations - */ - // TODO: Check if we actually need to do this... - // pose.transformBy(new Transform3d(new Translation3d(VisionConstants.kFieldLength, VisionConstants.kFieldWidth, 0.0), new Rotation3d())); - return Optional.of(new Measurement( timestamp, pose, diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 0000000..ff15fab --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2024.2.3", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2024", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2024.2.3" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2024.2.3", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 0f3520e..a829581 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.0", + "version": "2024.2.1", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.0" + "version": "2024.2.1" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.0", + "version": "2024.2.1", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.1", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false,