From 0f59608cca00480e56535cebfb11f33793a48774 Mon Sep 17 00:00:00 2001 From: mebrahimaleem Date: Sat, 6 Jan 2024 11:35:15 -0800 Subject: [PATCH 01/10] feat: Alignment logic --- src/main/java/frc/robot/Constants.java | 35 ++++++++ .../frc/robot/commands/AlignToTagCommand.java | 83 +++++++++++++++++++ 2 files changed, 118 insertions(+) create mode 100644 src/main/java/frc/robot/Constants.java create mode 100644 src/main/java/frc/robot/commands/AlignToTagCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..f0b9720 --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,35 @@ +package frc.robot; + +import edu.wpi.first.math.geometry.Pose2d; + +public final class Constants { + public final static class TagAlignConstants { + public final static double kTagPIDkPxy = 0; + public final static double kTagPIDkIxy = 0; + public final static double kTagPIDkDxy = 0; + + public final static double kTagPIDkPomega = 0; + public final static double kTagPIDkIomega = 0; + public final static double kTagPIDkDomega = 0; + + public static final Pose2d[] kTargetPoses = new Pose2d[] {}; // Poses are in the same order as the enumerator. + // TODO: populate poses + + //TODO: tune all these parameters + public static final double kTolxy = 0.1; // tolerance in meters for x and y + public static final double kTolomega = 0.1; // tolerance in radians for omega + public static final double kConsectol = 10; // onsecutive cycles of being within tolerance needed to end command + public static final double kMinProx = 1; // minimum proximity radius in meters. + // To avoid accidents, the robot must be + // within the minimum proximity for the tag alignment command to run. + // If the robot exceeds the minimum proximity the command will finish + + public static enum AlignPosition { // DON'T CHANGE THE ORDER OF THESE. Add new items to the end + Amp, + SourceLeft, + SourceCenter, + SourceRight, + Speaker + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AlignToTagCommand.java b/src/main/java/frc/robot/commands/AlignToTagCommand.java new file mode 100644 index 0000000..30991ac --- /dev/null +++ b/src/main/java/frc/robot/commands/AlignToTagCommand.java @@ -0,0 +1,83 @@ +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Subsystem; +import frc.robot.Constants.TagAlignConstants; +import frc.robot.Constants.TagAlignConstants.AlignPosition; + +public class AlignToTagCommand extends CommandBase { + + final Subsystem m_subsystem; // TODO: change type to drive subsystem + final Pose2d m_target; + final PIDController m_PIDxy; + final PIDController m_PIDomega; + + double m_consecCount; // counter of number of consecutive cycles within tolerance + + /** + * + * @param subsystem Subsystem to require. Subsystem must implement a drive + * method and a getPosition method. + * @param alignPos Position to align to + */ + public AlignToTagCommand(Subsystem subsystem, AlignPosition alignPos) { + m_subsystem = subsystem; + addRequirements(m_subsystem); + m_target = TagAlignConstants.kTargetPoses[alignPos.ordinal()]; + m_PIDxy = new PIDController(TagAlignConstants.kTagPIDkPxy, TagAlignConstants.kTagPIDkIxy, + TagAlignConstants.kTagPIDkDxy); + m_PIDomega = new PIDController(TagAlignConstants.kTagPIDkPomega, TagAlignConstants.kTagPIDkIomega, + TagAlignConstants.kTagPIDkDomega); + m_PIDomega.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void initialize() { + m_PIDxy.reset(); + m_PIDomega.reset(); + m_consecCount = 0; + } + + @Override + public void execute() { + Pose2d current = new Pose2d(); // TODO: get pose from april tag + + double xSpeed = MathUtil.clamp(m_PIDxy.calculate(current.getX(), m_target.getX()), + -0, 0); // TODO: change clamp parameters and units + + double ySpeed = MathUtil.clamp( + m_PIDxy.calculate(current.getY(), m_target.getY()), + -0, 0); + + double omegaSpeed = MathUtil.clamp( + m_PIDomega.calculate(current.getRotation().getRadians(), m_target.getRotation().getRadians()), + -0, 0); + + // Check if within tolerance + if (Math.abs(current.getX() - m_target.getX()) < TagAlignConstants.kTolxy && // x within range + Math.abs(current.getY() - m_target.getY()) < TagAlignConstants.kTolxy && // y within range + Math.abs( // omega within range + current.getRotation().getRadians() - m_target.getRotation().getRadians()) < TagAlignConstants.kTolomega) { + m_consecCount++; + } else + m_consecCount = 0; + + // TODO: call drive function + } + + @Override + public void end(boolean interrupted) { + // TODO: log finish reason to NT (either reached, proximity, or interrupted) + } + + @Override + public boolean isFinished() { + Pose2d current = new Pose2d(); // TODO: get pose from april tag + return Math.pow(current.getX() - m_target.getX(), 2) + Math.pow(current.getY() - m_target.getY(), 2) > Math + .pow(TagAlignConstants.kMinProx, 2) || // Exceed proximity + m_consecCount >= TagAlignConstants.kConsectol; // Reached setpoint + } +} \ No newline at end of file From e36997780342e9ab38e1a2e50dcf4e5a72baf7e3 Mon Sep 17 00:00:00 2001 From: mebrahimaleem Date: Sat, 6 Jan 2024 12:24:49 -0800 Subject: [PATCH 02/10] feat: uses poseEstimator + vision to get pose --- src/main/java/frc/robot/Constants.java | 6 +++ .../frc/robot/commands/AlignToTagCommand.java | 46 +++++++++++++++++-- 2 files changed, 49 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f0b9720..7b2968e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,5 +1,6 @@ package frc.robot; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; public final class Constants { @@ -32,4 +33,9 @@ public static enum AlignPosition { // DON'T CHANGE THE ORDER OF THESE. Add new i Speaker } } + + public final static class DriveConstants { + //TODO: init pose estimator with actual values + public static final SwerveDrivePoseEstimator kPoseEstimator = new SwerveDrivePoseEstimator(null, null, null, null); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AlignToTagCommand.java b/src/main/java/frc/robot/commands/AlignToTagCommand.java index 30991ac..31f915d 100644 --- a/src/main/java/frc/robot/commands/AlignToTagCommand.java +++ b/src/main/java/frc/robot/commands/AlignToTagCommand.java @@ -3,14 +3,21 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.Subsystem; +import frc.robot.Constants.DriveConstants; import frc.robot.Constants.TagAlignConstants; import frc.robot.Constants.TagAlignConstants.AlignPosition; public class AlignToTagCommand extends CommandBase { - final Subsystem m_subsystem; // TODO: change type to drive subsystem + final Subsystem m_subsystem; // TODO: change type to drive subsystem type final Pose2d m_target; final PIDController m_PIDxy; final PIDController m_PIDomega; @@ -43,7 +50,7 @@ public void initialize() { @Override public void execute() { - Pose2d current = new Pose2d(); // TODO: get pose from april tag + Pose2d current = poseFromAprilTag(); double xSpeed = MathUtil.clamp(m_PIDxy.calculate(current.getX(), m_target.getX()), -0, 0); // TODO: change clamp parameters and units @@ -75,9 +82,42 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - Pose2d current = new Pose2d(); // TODO: get pose from april tag + Pose2d current = poseFromAprilTag(); return Math.pow(current.getX() - m_target.getX(), 2) + Math.pow(current.getY() - m_target.getY(), 2) > Math .pow(TagAlignConstants.kMinProx, 2) || // Exceed proximity m_consecCount >= TagAlignConstants.kConsectol; // Reached setpoint } + + /** + * Gets pose from vision measurements from limelight + * + * @return Robot Pose + */ + Pose2d poseFromAprilTag() { // TODO: move this to drive subsystem + NetworkTable tb = NetworkTableInstance.getDefault().getTable("limelight"); + DoubleArrayEntry botpose = null; // index 0: x, 1: y, 2: z, 3: roll, 4: pitch, 5: yaw, 6: timestamp. all angles + // are in degrees + + try { + botpose = tb.getDoubleArrayTopic("botpose").getEntry(new double[0]); + double[] bp = botpose.getAtomic().value; // Not sure how threading works in WPIlib w/ command scheduler. If it is + // thread safe then atomic wont be needed. There is no need to use the + // atomic timestamp since the topic has the timestamp stored in it (7th + // element) + + if (botpose.exists()) { + Pose2d visionPose = new Pose2d(new Translation2d(bp[0], bp[1]), new Rotation2d(bp[5] * Math.PI / 180)); + DriveConstants.kPoseEstimator.addVisionMeasurement(visionPose, Timer.getFPGATimestamp() - (bp[6] / 1000)); + } else { + // TODO: log limelight failure + } + + } finally { + if (botpose != null && botpose.isValid()) + botpose.close(); // TODO: verify this logic: make sure it will never throw an exception, + // otherwise add another trycatch block + } + + return DriveConstants.kPoseEstimator.getEstimatedPosition(); + } } \ No newline at end of file From c677ac323b181603903a922123e711c80c91cda6 Mon Sep 17 00:00:00 2001 From: mebrahimaleem Date: Sat, 6 Jan 2024 12:45:52 -0800 Subject: [PATCH 03/10] feat: swerve code from swerve-base --- src/main/java/frc/robot/Constants.java | 93 ++++ src/main/java/frc/robot/RobotContainer.java | 57 ++- .../frc/robot/subsystems/DriveSubsystem.java | 226 ++++++++++ .../frc/robot/subsystems/SwerveModule.java | 104 +++++ vendordeps/NavX.json | 39 ++ vendordeps/Phoenix.json | 423 ++++++++++++++++++ vendordeps/REVLib.json | 73 +++ 7 files changed, 1014 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/Constants.java create mode 100644 src/main/java/frc/robot/subsystems/DriveSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/SwerveModule.java create mode 100644 vendordeps/NavX.json create mode 100644 vendordeps/Phoenix.json create mode 100644 vendordeps/REVLib.json diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..0da951c --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,93 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean + * constants. This class should not be used for any other purpose. All constants + * should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *

+ * It is advised to statically import this class (or one of its inner classes) + * wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + + /** + * Input/Output constants + */ + public static 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 = 1; + public static final int kRearLeftDriveMotorPort = 3; + public static final int kFrontRightDriveMotorPort = 5; + public static final int kRearRightDriveMotorPort = 7; + + public static final int kFrontLeftTurningMotorPort = 2; + public static final int kRearLeftTurningMotorPort = 4; + public static final int kFrontRightTurningMotorPort = 6; + public static final int kRearRightTurningMotorPort = 8; + + public static final int kFrontLeftTurningEncoderPort = 9; + public static final int kRearLeftTurningEncoderPort = 10; + public static final int kFrontRightTurningEncoderPort = 11; + public static final int kRearRightTurningEncoderPort = 12; + + 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.5; + + /** Distance between front and back wheels on robot (in meters). */ + public static final double kWheelBase = 0.5; + + /** 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 + + // 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 Neos */ + public static final double kMaxSpeedMetersPerSecond = 3.6576; + /** For a a SDS Mk4i L1 swerve base with Neos */ + public static final double kMaxAngularSpeedRadiansPerSecond = 15.24 / 3; + + /** 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; + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d0..07889d9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,15 +4,70 @@ package frc.robot; +import edu.wpi.first.math.MathUtil; +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; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.IOConstants; +import frc.robot.subsystems.DriveSubsystem; + public class RobotContainer { + // The robot's subsystems and commands are defined here + private final DriveSubsystem m_robotDrive = new DriveSubsystem(); + + private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ public RobotContainer() { + // Configure the trigger bindings configureBindings(); + + m_robotDrive.setDefaultCommand( + new RunCommand( + () -> m_robotDrive.drive( + MathUtil.applyDeadband( + -m_driverController.getLeftY(), + IOConstants.kControllerDeadband) + * DriveConstants.kMaxSpeedMetersPerSecond + * (1 - m_driverController + .getLeftTriggerAxis() + * IOConstants.kSlowModeScalar) + * 0.8, + MathUtil.applyDeadband( + -m_driverController.getLeftX(), + IOConstants.kControllerDeadband) + * DriveConstants.kMaxSpeedMetersPerSecond + * (1 - m_driverController + .getLeftTriggerAxis() + * IOConstants.kSlowModeScalar) + * 0.8, + MathUtil.applyDeadband( + m_driverController.getRightX(), + IOConstants.kControllerDeadband) + * DriveConstants.kMaxAngularSpeedRadiansPerSecond + * (1 - m_driverController + .getLeftTriggerAxis() + * IOConstants.kSlowModeScalar) + / 2, + !m_driverController.getRightBumper()), + m_robotDrive)); } - private void configureBindings() {} + /** + * Use this method to define your button->command mappings. + */ + private void configureBindings() { + new JoystickButton(m_driverController, Button.kStart.value) + .onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive)); + } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java new file mode 100644 index 0000000..aebcf73 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -0,0 +1,226 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.kauailabs.navx.frc.AHRS; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.DriveConstants; +import frc.robot.Robot; + +public class DriveSubsystem extends SubsystemBase { + private final SwerveModule m_frontLeft = new SwerveModule( + DriveConstants.kFrontLeftDriveMotorPort, + DriveConstants.kFrontLeftTurningMotorPort, + DriveConstants.kFrontLeftTurningEncoderPort, + DriveConstants.kFrontLeftDriveMotorReversed, + DriveConstants.kFrontLeftTurningEncoderOffset); + + private final SwerveModule m_rearLeft = new SwerveModule( + DriveConstants.kRearLeftDriveMotorPort, + DriveConstants.kRearLeftTurningMotorPort, + DriveConstants.kRearLeftTurningEncoderPort, + DriveConstants.kRearLeftDriveMotorReversed, + DriveConstants.kRearLeftTurningEncoderOffset); + + private final SwerveModule m_frontRight = new SwerveModule( + DriveConstants.kFrontRightDriveMotorPort, + DriveConstants.kFrontRightTurningMotorPort, + DriveConstants.kFrontRightTurningEncoderPort, + DriveConstants.kFrontRightDriveMotorReversed, + DriveConstants.kFrontRightTurningEncoderOffset); + + private final SwerveModule m_rearRight = new SwerveModule( + DriveConstants.kRearRightDriveMotorPort, + DriveConstants.kRearRightTurningMotorPort, + DriveConstants.kRearRightTurningEncoderPort, + DriveConstants.kRearRightDriveMotorReversed, + DriveConstants.kRearRightTurningEncoderOffset); + + private final AHRS m_gyro = new AHRS(); + private double m_gyroAngle; + + private final Timer m_headingCorrectionTimer = new Timer(); + private final PIDController m_headingCorrectionPID = new PIDController(DriveConstants.kPHeadingCorrectionController, + 0, 0); + private SwerveModulePosition[] m_swerveModulePositions = new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition() + }; + + // TODO: Experiment with different std devs in the pose estimator + private final SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator(DriveConstants.kDriveKinematics, + m_gyro.getRotation2d(), m_swerveModulePositions, new Pose2d()); + + private final Field2d m_field = new Field2d(); + + /** Creates a new DriveSubsystem. */ + public DriveSubsystem() { + SmartDashboard.putData("Field", m_field); + m_headingCorrectionTimer.restart(); + m_headingCorrectionPID.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + + m_swerveModulePositions = new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition() + }; + + m_poseEstimator.update(Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle), m_swerveModulePositions); + + m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); + + SmartDashboard.putNumber("gyro angle", m_gyro.getAngle()); + SmartDashboard.putNumber("odometryX", m_poseEstimator.getEstimatedPosition().getX()); + SmartDashboard.putNumber("odometryY", m_poseEstimator.getEstimatedPosition().getY()); + + // AdvantageScope Logging + double[] logData = { + m_frontLeft.getPosition().angle.getDegrees(), m_frontLeft.driveOutput, + m_frontRight.getPosition().angle.getDegrees(), m_frontRight.driveOutput, + m_rearLeft.getPosition().angle.getDegrees(), m_rearLeft.driveOutput, + m_rearRight.getPosition().angle.getDegrees(), m_rearRight.driveOutput, + }; + SmartDashboard.putNumberArray("AdvantageScope Swerve States", logData); + } + + /** + * Returns the currently-estimated pose of the robot. + * + * @return The pose. + */ + public Pose2d getPose() { + return m_poseEstimator.getEstimatedPosition(); + } + + /** + * Method to drive the robot using joystick info. + * + * @param xSpeed Speed of the robot in the x direction (forward). + * @param ySpeed Speed of the robot in the y direction (sideways). + * @param rotation Angular rotation speed of the robot. + * @param fieldRelative Whether the provided x and y speeds are relative to the + * field. + */ + public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) { + // If we are rotating, reset the timer + if (rotation != 0) { + m_headingCorrectionTimer.reset(); + } + + /* + * Heading correction helps maintain the same heading and + * prevents rotational drive while our robot is translating + * + * For heading correction we use a timer to ensure that we + * lose all rotational momentum before saving the heading + * that we want to maintain + */ + + // TODO: Test heading correction without timer + // TODO: Test heading correction using gyro's rotational velocity (if it is 0 + // then set heading instead of timer) + + // Save our desired rotation to a variable we can add our heading correction + // adjustments to + double calculatedRotation = rotation; + + double currentAngle = MathUtil.angleModulus(m_gyro.getRotation2d().getRadians()); + + // If we are not translating or if not enough time has passed since the last + // time we rotated + if ((xSpeed == 0 && ySpeed == 0) + || m_headingCorrectionTimer.get() < DriveConstants.kHeadingCorrectionTurningStopTime) { + // Update our desired angle + m_headingCorrectionPID.setSetpoint(currentAngle); + } else { + // If we are translating or if we have not rotated for a long enough time + // then maintain our desired angle + calculatedRotation = m_headingCorrectionPID.calculate(currentAngle); + } + + // 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( + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, calculatedRotation, + Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle)) + : new ChassisSpeeds(xSpeed, ySpeed, calculatedRotation)); + + setModuleStates(swerveModuleStates); + } + + /** + * Resets the odometry to the specified pose. + * + * @param pose The pose to which to set the odometry. + */ + public void resetOdometry(Pose2d pose) { + m_poseEstimator.resetPosition( + Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle), + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition() + }, + pose); + } + + /** Zeroes the heading of the robot. */ + public void zeroHeading() { + m_gyro.reset(); + m_gyroAngle = 0; + } + + /** + * Sets the swerve ModuleStates. + * + * @param desiredStates The desired SwerveModule states. + */ + public void setModuleStates(SwerveModuleState[] desiredStates) { + SwerveDriveKinematics.desaturateWheelSpeeds( + desiredStates, DriveConstants.kMaxSpeedMetersPerSecond); + m_frontLeft.setDesiredState(desiredStates[0]); + m_frontRight.setDesiredState(desiredStates[1]); + m_rearLeft.setDesiredState(desiredStates[2]); + m_rearRight.setDesiredState(desiredStates[3]); + + // AdvantageScope Logging + double[] logData = { + desiredStates[0].angle.getDegrees(), desiredStates[0].speedMetersPerSecond, + desiredStates[1].angle.getDegrees(), desiredStates[1].speedMetersPerSecond, + desiredStates[2].angle.getDegrees(), desiredStates[2].speedMetersPerSecond, + desiredStates[3].angle.getDegrees(), desiredStates[3].speedMetersPerSecond, + }; + SmartDashboard.putNumberArray("AdvantageScope Swerve Desired States", logData); + + // Takes the integral of the rotation speed to find the current angle for the + // simulator + m_gyroAngle += DriveConstants.kDriveKinematics.toChassisSpeeds(desiredStates).omegaRadiansPerSecond + * Robot.kDefaultPeriod; + } + +} diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java new file mode 100644 index 0000000..9647e5b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -0,0 +1,104 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.SensorTimeBase; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import frc.robot.Constants.DriveConstants; +import frc.robot.Robot; + +/** Add your docs here. */ +public class SwerveModule { + private final CANSparkMax m_driveMotor; + private final CANSparkMax m_turningMotor; + + private final CANCoder m_turningEncoder; + + private final PIDController m_turningPIDController = new PIDController(DriveConstants.kPModuleTurningController, 0, + 0); + + private SwerveModuleState m_state = new SwerveModuleState(); + private double m_distance; + + public double driveOutput; + public double turnOutput; + + /** + * 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. + */ + 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); + m_turningEncoder = new CANCoder(turningEncoderPort); + + // converts default units to meters per second + m_driveMotor.getEncoder().setVelocityConversionFactor( + DriveConstants.kWheelDiameterMeters * Math.PI / 60 / DriveConstants.kDrivingGearRatio); + + m_driveMotor.setInverted(driveMotorReversed); + + m_turningMotor.setIdleMode(IdleMode.kBrake); + + // converts default units of CANCoders to radians + m_turningEncoder.configFeedbackCoefficient(Math.toRadians(0.087890625), "radians", SensorTimeBase.PerSecond); + m_turningEncoder.configMagnetOffset(-turningEncoderOffset); + + m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI); + } + + /** + * Returns the current position of the module. + * + * @return The current position of the module. + */ + public SwerveModulePosition getPosition() { + m_distance += m_state.speedMetersPerSecond * Robot.kDefaultPeriod; + + // If the robot is real, then return the swerve module state by reading from the + // actual encoders + // If the robot is simulated, then return the swerve module state using the + // expected values + return Robot.isReal() + ? new SwerveModulePosition(m_driveMotor.getEncoder().getPosition(), + new Rotation2d(m_turningEncoder.getAbsolutePosition())) + : new SwerveModulePosition(m_distance, m_state.angle); + } + + /** + * Sets the desired state for the module. + * + * @param desiredState Desired state with speed and angle. + */ + public void setDesiredState(SwerveModuleState desiredState) { + m_state = SwerveModuleState.optimize(desiredState, new Rotation2d(m_turningEncoder.getAbsolutePosition())); + + driveOutput = m_state.speedMetersPerSecond / DriveConstants.kMaxSpeedMetersPerSecond; + + turnOutput = m_turningPIDController.calculate(m_turningEncoder.getAbsolutePosition(), + m_state.angle.getRadians()); + + m_driveMotor.set(driveOutput); + m_turningMotor.set(turnOutput); + } +} diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json new file mode 100644 index 0000000..33d81f6 --- /dev/null +++ b/vendordeps/NavX.json @@ -0,0 +1,39 @@ +{ + "fileName": "NavX.json", + "name": "KauaiLabs_navX_FRC", + "version": "2023.0.4", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2023/" + ], + "jsonUrl": "https://dev.studica.com/releases/2023/NavX.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-java", + "version": "2023.0.4" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-cpp", + "version": "2023.0.4", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json new file mode 100644 index 0000000..614dc3a --- /dev/null +++ b/vendordeps/Phoenix.json @@ -0,0 +1,423 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.31.0+23.2.2", + "frcYear": 2023, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.31.0" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.31.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.31.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.31.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "23.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "23.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "23.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "23.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "23.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "23.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "23.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "23.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "23.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "23.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.31.0", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.31.0", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.31.0", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "23.2.2", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.31.0", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.31.0", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.31.0", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "23.2.2", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "23.2.2", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "23.2.2", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "23.2.2", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "23.2.2", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "23.2.2", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "23.2.2", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "23.2.2", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "23.2.2", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..f2d0b7d --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,73 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2023.1.3", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2023.1.3" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2023.1.3", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2023.1.3", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2023.1.3", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From 8c0d973a290fc18047a1604032845e03159aa85c Mon Sep 17 00:00:00 2001 From: mebrahimaleem Date: Sat, 6 Jan 2024 13:18:50 -0800 Subject: [PATCH 04/10] feat: added placeholders for more align positions --- src/main/java/frc/robot/Constants.java | 32 +++++++++++++++++++++++--- 1 file changed, 29 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7b2968e..91bf4e6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -2,6 +2,8 @@ import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; public final class Constants { public final static class TagAlignConstants { @@ -13,7 +15,22 @@ public final static class TagAlignConstants { public final static double kTagPIDkIomega = 0; public final static double kTagPIDkDomega = 0; - public static final Pose2d[] kTargetPoses = new Pose2d[] {}; // Poses are in the same order as the enumerator. + public static final Pose2d[] kTargetPoses = new Pose2d[] { + new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), //Amp + new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), //SourceLeft + new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), //etc. + new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), + new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), + new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), + new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), + new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), + new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), + new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), + new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), + new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), + new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), + new Pose2d(new Translation2d(0, 0), new Rotation2d(0)) //StageCenterRight + }; // Poses are in the same order as the enumerator. // TODO: populate poses //TODO: tune all these parameters @@ -30,8 +47,17 @@ public static enum AlignPosition { // DON'T CHANGE THE ORDER OF THESE. Add new i SourceLeft, SourceCenter, SourceRight, - Speaker - } + Speaker, + StageLeftLeft, // The left side of the left stage + StageLeftCenter, // the center side of the left stage + StageLeftRight, // etc. + StageRightLeft, + StageRightCenter, + StageRightRight, + StageCenterLeft, + StageCenterCenter, + StageCenterRight // the right side of the center stage + } //stage left, stage right, and stage center are the locations written in the game manual pg. 28 } public final static class DriveConstants { From 36147a0de64f4e97f6ec05e074b491ab847383c3 Mon Sep 17 00:00:00 2001 From: mebrahimaleem Date: Sat, 6 Jan 2024 17:54:36 -0800 Subject: [PATCH 05/10] fix: Corrected logic on target pose calculation --- src/main/java/frc/robot/Constants.java | 16 +++++++- .../frc/robot/commands/AlignToTagCommand.java | 38 ++++++++++++++----- 2 files changed, 42 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 91bf4e6..9ae3911 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,14 +4,15 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableInstance; public final class Constants { public final static class TagAlignConstants { - public final static double kTagPIDkPxy = 0; + public final static double kTagPIDkPxy = 0; // xy PID constants public final static double kTagPIDkIxy = 0; public final static double kTagPIDkDxy = 0; - public final static double kTagPIDkPomega = 0; + public final static double kTagPIDkPomega = 0; // omega PID constants public final static double kTagPIDkIomega = 0; public final static double kTagPIDkDomega = 0; @@ -58,6 +59,17 @@ public static enum AlignPosition { // DON'T CHANGE THE ORDER OF THESE. Add new i StageCenterCenter, StageCenterRight // the right side of the center stage } //stage left, stage right, and stage center are the locations written in the game manual pg. 28 + + public static final double kFieldWidth = 651.25 * 0.0254; // Width of the field in meters + + public static final NetworkTableInstance kNTInstance = NetworkTableInstance.create(); // Ideally all network table + // instances should be put in + // a wrapper and only the + // default instance should be + // used. + // TODO: write unit tests that write to NT limelight and verify PID output + // direction is correct. Needs to be implemented after drive since it uses pose + // estimator. } public final static class DriveConstants { diff --git a/src/main/java/frc/robot/commands/AlignToTagCommand.java b/src/main/java/frc/robot/commands/AlignToTagCommand.java index 31f915d..d3cb451 100644 --- a/src/main/java/frc/robot/commands/AlignToTagCommand.java +++ b/src/main/java/frc/robot/commands/AlignToTagCommand.java @@ -7,8 +7,9 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.DoubleArrayEntry; import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.Constants.DriveConstants; @@ -18,9 +19,9 @@ public class AlignToTagCommand extends CommandBase { final Subsystem m_subsystem; // TODO: change type to drive subsystem type - final Pose2d m_target; + final Pose2d m_target; // Desired pose final PIDController m_PIDxy; - final PIDController m_PIDomega; + final PIDController m_PIDomega; // Omega is used to denote robot heading in radians double m_consecCount; // counter of number of consecutive cycles within tolerance @@ -33,17 +34,31 @@ public class AlignToTagCommand extends CommandBase { public AlignToTagCommand(Subsystem subsystem, AlignPosition alignPos) { m_subsystem = subsystem; addRequirements(m_subsystem); - m_target = TagAlignConstants.kTargetPoses[alignPos.ordinal()]; + + final Pose2d redTarget = TagAlignConstants.kTargetPoses[alignPos.ordinal()]; + + // Blue positions are determined by flipping the x position and angle across the + // center. If this logic does not work, then hard coded values may be better + // (and faster). Indexing can be done by adding a constant to the ordinal when + // indexing the array. This will allows accessing the blue positions which + // should be put in the array after the red ones. + m_target = DriverStation.getAlliance().compareTo(Alliance.Red) == 0 ? redTarget + : new Pose2d(new Translation2d( // Flip poses of alliance is blue + TagAlignConstants.kFieldWidth - redTarget.getX(), + redTarget.getY()), + new Rotation2d(MathUtil.angleModulus(Math.PI - redTarget.getRotation().getRadians()))); + m_PIDxy = new PIDController(TagAlignConstants.kTagPIDkPxy, TagAlignConstants.kTagPIDkIxy, - TagAlignConstants.kTagPIDkDxy); + TagAlignConstants.kTagPIDkDxy); // A feed forward control might also be needed here to overcome static friction m_PIDomega = new PIDController(TagAlignConstants.kTagPIDkPomega, TagAlignConstants.kTagPIDkIomega, TagAlignConstants.kTagPIDkDomega); + m_PIDomega.enableContinuousInput(-Math.PI, Math.PI); } @Override public void initialize() { - m_PIDxy.reset(); + m_PIDxy.reset(); // If a command instance is never reused then these three lines won't be needed m_PIDomega.reset(); m_consecCount = 0; } @@ -57,11 +72,11 @@ public void execute() { double ySpeed = MathUtil.clamp( m_PIDxy.calculate(current.getY(), m_target.getY()), - -0, 0); + -0, 0); // TODO: change clamp parameters and units double omegaSpeed = MathUtil.clamp( m_PIDomega.calculate(current.getRotation().getRadians(), m_target.getRotation().getRadians()), - -0, 0); + -0, 0); // TODO: change clamp parameters and units // Check if within tolerance if (Math.abs(current.getX() - m_target.getX()) < TagAlignConstants.kTolxy && // x within range @@ -78,13 +93,15 @@ public void execute() { @Override public void end(boolean interrupted) { // TODO: log finish reason to NT (either reached, proximity, or interrupted) + // This is also a good place to start an instant command to move the outake to + // the correct position } @Override public boolean isFinished() { Pose2d current = poseFromAprilTag(); return Math.pow(current.getX() - m_target.getX(), 2) + Math.pow(current.getY() - m_target.getY(), 2) > Math - .pow(TagAlignConstants.kMinProx, 2) || // Exceed proximity + .pow(TagAlignConstants.kMinProx, 2) || // Exceed proximity. m_consecCount >= TagAlignConstants.kConsectol; // Reached setpoint } @@ -94,7 +111,8 @@ public boolean isFinished() { * @return Robot Pose */ Pose2d poseFromAprilTag() { // TODO: move this to drive subsystem - NetworkTable tb = NetworkTableInstance.getDefault().getTable("limelight"); + NetworkTable tb = TagAlignConstants.kNTInstance.getTable("limelight"); + DoubleArrayEntry botpose = null; // index 0: x, 1: y, 2: z, 3: roll, 4: pitch, 5: yaw, 6: timestamp. all angles // are in degrees From 7eee9bb3dadaa1b24a7400d638b43717fd6e4dd3 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Mon, 8 Jan 2024 16:42:36 -0800 Subject: [PATCH 06/10] Changed motor controller to SparkFlex --- .../java/frc/robot/subsystems/SwerveModule.java | 14 +++++++------- vendordeps/REVLib.json | 13 +++++++------ 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 9647e5b..2651af0 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -6,9 +6,9 @@ import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.SensorTimeBase; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; @@ -19,8 +19,8 @@ /** Add your docs here. */ 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; @@ -48,8 +48,8 @@ public SwerveModule( int turningEncoderPort, boolean driveMotorReversed, double turningEncoderOffset) { - m_driveMotor = new CANSparkMax(driveMotorPort, MotorType.kBrushless); - m_turningMotor = new CANSparkMax(turningMotorPort, MotorType.kBrushless); + m_driveMotor = new CANSparkFlex(driveMotorPort, MotorType.kBrushless); + m_turningMotor = new CANSparkFlex(turningMotorPort, MotorType.kBrushless); m_turningEncoder = new CANCoder(turningEncoderPort); // converts default units to meters per second diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index f2d0b7d..0f3520e 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,24 +1,25 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2023.1.3", + "version": "2024.2.0", + "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "https://maven.revrobotics.com/" ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json", + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", "javaDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2023.1.3" + "version": "2024.2.0" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2023.1.3", + "version": "2024.2.0", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2023.1.3", + "version": "2024.2.0", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -54,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2023.1.3", + "version": "2024.2.0", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, From 629902545aac2273a399aadbb60ff6aab098ed4a Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Mon, 8 Jan 2024 18:54:51 -0800 Subject: [PATCH 07/10] tbh idk --- networktables.json | 1 + simgui-ds.json | 92 ++++++++++++++++++++++++++++++++++++++++++++++ simgui.json | 7 ++++ 3 files changed, 100 insertions(+) create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..449f4b1 --- /dev/null +++ b/simgui.json @@ -0,0 +1,7 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo" + } + } +} From 2bfaef86907dcb38c48f337c9e4dc393b22cd58e Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Fri, 19 Jan 2024 17:03:39 -0800 Subject: [PATCH 08/10] feat: imported to 2024 --- build.gradle | 2 +- networktables.json | 1 + simgui-ds.json | 97 ++++++++++ simgui.json | 18 ++ src/main/java/frc/robot/Constants.java | 2 +- .../frc/robot/subsystems/SwerveModule.java | 24 +-- vendordeps/NavX.json | 13 +- vendordeps/{Phoenix.json => Phoenix6.json} | 166 +++++------------- 8 files changed, 179 insertions(+), 144 deletions(-) create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json rename vendordeps/{Phoenix.json => Phoenix6.json} (69%) diff --git a/build.gradle b/build.gradle index 1478f2b..d141f06 100644 --- a/build.gradle +++ b/build.gradle @@ -68,7 +68,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + //testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } test { diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..69b1a3c --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,97 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..905d0ec --- /dev/null +++ b/simgui.json @@ -0,0 +1,18 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Field": "Field2d" + }, + "windows": { + "/SmartDashboard/Field": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0da951c..ddfa1bf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -72,7 +72,7 @@ public static final class DriveConstants { public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 configuration // TODO: Tune this PID before running on a robot on the ground - public static final double kPModuleTurningController = -0.3; + public static final double kPModuleTurningController = 0.3; public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( new Translation2d(kWheelBase / 2, kTrackWidth / 2), diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 2651af0..40578b8 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -4,8 +4,8 @@ package frc.robot.subsystems; -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.SensorTimeBase; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -14,6 +14,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.Units; import frc.robot.Constants.DriveConstants; import frc.robot.Robot; @@ -22,7 +23,7 @@ public class SwerveModule { private final CANSparkFlex m_driveMotor; private final CANSparkFlex m_turningMotor; - private final CANCoder m_turningEncoder; + private final CANcoder m_turningEncoder; private final PIDController m_turningPIDController = new PIDController(DriveConstants.kPModuleTurningController, 0, 0); @@ -50,7 +51,7 @@ public SwerveModule( double turningEncoderOffset) { m_driveMotor = new CANSparkFlex(driveMotorPort, MotorType.kBrushless); m_turningMotor = new CANSparkFlex(turningMotorPort, MotorType.kBrushless); - m_turningEncoder = new CANCoder(turningEncoderPort); + m_turningEncoder = new CANcoder(turningEncoderPort); // converts default units to meters per second m_driveMotor.getEncoder().setVelocityConversionFactor( @@ -59,10 +60,8 @@ public SwerveModule( m_driveMotor.setInverted(driveMotorReversed); m_turningMotor.setIdleMode(IdleMode.kBrake); - - // converts default units of CANCoders to radians - m_turningEncoder.configFeedbackCoefficient(Math.toRadians(0.087890625), "radians", SensorTimeBase.PerSecond); - m_turningEncoder.configMagnetOffset(-turningEncoderOffset); + + m_turningEncoder.getConfigurator().apply(new CANcoderConfiguration().MagnetSensor.withMagnetOffset(-turningEncoderOffset)); m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI); } @@ -79,9 +78,12 @@ public SwerveModulePosition getPosition() { // actual encoders // If the robot is simulated, then return the swerve module state using the // expected values + + + ; return Robot.isReal() ? new SwerveModulePosition(m_driveMotor.getEncoder().getPosition(), - new Rotation2d(m_turningEncoder.getAbsolutePosition())) + new Rotation2d(Units.RadiansPerSecond.convertFrom(m_turningEncoder.getAbsolutePosition().getValueAsDouble(), Units.RotationsPerSecond))) : new SwerveModulePosition(m_distance, m_state.angle); } @@ -91,11 +93,11 @@ public SwerveModulePosition getPosition() { * @param desiredState Desired state with speed and angle. */ public void setDesiredState(SwerveModuleState desiredState) { - m_state = SwerveModuleState.optimize(desiredState, new Rotation2d(m_turningEncoder.getAbsolutePosition())); + m_state = SwerveModuleState.optimize(desiredState, new Rotation2d(Units.RadiansPerSecond.convertFrom(m_turningEncoder.getAbsolutePosition().getValueAsDouble(), Units.RotationsPerSecond))); driveOutput = m_state.speedMetersPerSecond / DriveConstants.kMaxSpeedMetersPerSecond; - turnOutput = m_turningPIDController.calculate(m_turningEncoder.getAbsolutePosition(), + turnOutput = m_turningPIDController.calculate(Units.RadiansPerSecond.convertFrom(m_turningEncoder.getAbsolutePosition().getValueAsDouble(), Units.RotationsPerSecond), m_state.angle.getRadians()); m_driveMotor.set(driveOutput); diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json index 33d81f6..e978a5f 100644 --- a/vendordeps/NavX.json +++ b/vendordeps/NavX.json @@ -1,17 +1,18 @@ { "fileName": "NavX.json", - "name": "KauaiLabs_navX_FRC", - "version": "2023.0.4", + "name": "NavX", + "version": "2024.1.0", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2024", "mavenUrls": [ - "https://dev.studica.com/maven/release/2023/" + "https://dev.studica.com/maven/release/2024/" ], - "jsonUrl": "https://dev.studica.com/releases/2023/NavX.json", + "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", "javaDependencies": [ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-frc-java", - "version": "2023.0.4" + "version": "2024.1.0" } ], "jniDependencies": [], @@ -19,7 +20,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-frc-cpp", - "version": "2023.0.4", + "version": "2024.1.0", "headerClassifier": "headers", "sourcesClassifier": "sources", "sharedLibrary": false, diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix6.json similarity index 69% rename from vendordeps/Phoenix.json rename to vendordeps/Phoenix6.json index 614dc3a..69a4079 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix6.json @@ -1,56 +1,32 @@ { - "fileName": "Phoenix.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.31.0+23.2.2", - "frcYear": 2023, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.1.0", + "frcYear": 2024, + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json", - "javaDependencies": [ + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "conflictsWith": [ { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.31.0" - }, + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], + "javaDependencies": [ { - "groupId": "com.ctre.phoenix", + "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "5.31.0" + "version": "24.1.0" } ], "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.31.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.31.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.2.2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -63,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.2.2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -76,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.2.2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -89,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.2.2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -102,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.2.2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -115,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.2.2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -128,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.2.2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -141,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.2.2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -154,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.2.2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -167,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.2.2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,40 +156,10 @@ ], "cppDependencies": [ { - "groupId": "com.ctre.phoenix", + "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "5.31.0", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.31.0", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.31.0", - "libName": "CTRE_PhoenixCCI", + "version": "24.1.0", + "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -227,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.2.2", + "version": "24.1.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,40 +186,10 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenix.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.31.0", - "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.31.0", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.31.0", - "libName": "CTRE_PhoenixCCISim", + "version": "24.1.0", + "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -287,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.2.2", + "version": "24.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -302,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.2.2", + "version": "24.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -317,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.2.2", + "version": "24.1.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -332,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.2.2", + "version": "24.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -347,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.2.2", + "version": "24.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -362,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.2.2", + "version": "24.1.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -377,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.2.2", + "version": "24.1.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -392,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.2.2", + "version": "24.1.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -407,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.2.2", + "version": "24.1.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, From 650e4b382ccedcf9ee0df2ba1aac0e5df49b28f5 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Fri, 19 Jan 2024 17:15:52 -0800 Subject: [PATCH 09/10] feat: Imported to 2024 --- src/main/java/frc/robot/Constants.java | 2 -- src/main/java/frc/robot/commands/AlignToTagCommand.java | 9 +++------ 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4d16b93..72ee1dc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,11 +1,9 @@ package frc.robot; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; /** diff --git a/src/main/java/frc/robot/commands/AlignToTagCommand.java b/src/main/java/frc/robot/commands/AlignToTagCommand.java index 1840679..cf02d5e 100644 --- a/src/main/java/frc/robot/commands/AlignToTagCommand.java +++ b/src/main/java/frc/robot/commands/AlignToTagCommand.java @@ -6,16 +6,13 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.robot.Constants.DriveConstants; +import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.TagAlignConstants; import frc.robot.Constants.TagAlignConstants.AlignPosition; import frc.robot.subsystems.DriveSubsystem; -public class AlignToTagCommand extends CommandBase { +public class AlignToTagCommand extends Command { final DriveSubsystem m_subsystem; final Pose2d m_target; // Desired pose @@ -40,7 +37,7 @@ public AlignToTagCommand(DriveSubsystem subsystem, AlignPosition alignPos) { // (and faster). Indexing can be done by adding a constant to the ordinal when // indexing the array. This will allows accessing the blue positions which // should be put in the array after the red ones. - m_target = DriverStation.getAlliance().compareTo(Alliance.Red) == 0 ? redTarget + m_target = DriverStation.getAlliance().get().compareTo(Alliance.Red) == 0 ? redTarget : new Pose2d(new Translation2d( // Flip poses of alliance is blue TagAlignConstants.kFieldWidth - redTarget.getX(), redTarget.getY()), From acacb7f1b2959c30bab21628aac7db310c0e5f90 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Sat, 20 Jan 2024 10:53:53 -0800 Subject: [PATCH 10/10] Feat: Field allignment positions added --- src/main/java/frc/robot/Constants.java | 32 +++++++++++++++----------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 72ee1dc..882171d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -100,20 +100,24 @@ public final static class TagAlignConstants { public final static double kTagPIDkDomega = 0; public static final Pose2d[] kTargetPoses = new Pose2d[] { - new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), //Amp - new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), //SourceLeft - new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), //etc. - new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), - new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), - new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), - new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), - new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), - new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), - new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), - new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), - new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), - new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), - new Pose2d(new Translation2d(0, 0), new Rotation2d(0)) //StageCenterRight + new Pose2d(new Translation2d(585.75, 313), new Rotation2d(Math.toRadians(270))), //Amp //Change y based on length of robot using 28in long, Red side + + //Asumed that these are positions for a single source, so that you have 3 positions for the source on the left + //Left , center, right, all on same obj //TODO: Make sure these work with robot demensions/bumpers + new Pose2d(new Translation2d(590, 9), new Rotation2d(Math.toRadians(120))), //SourceLeft + new Pose2d(new Translation2d(631.125, 20.625), new Rotation2d(Math.toRadians(120))), //etc. + new Pose2d(new Translation2d(637, 34), new Rotation2d(Math.toRadians(120))), + + new Pose2d(new Translation2d(598.125, 222.75), new Rotation2d(0)), //TODO: Check this (Red speaker) + new Pose2d(new Translation2d(464.5, 117.315), new Rotation2d(Math.toRadians(300))), + new Pose2d(new Translation2d(421.115, 161.62), new Rotation2d(Math.toRadians(180))), + new Pose2d(new Translation2d(459.75, 197.725), new Rotation2d(Math.toRadians(60))), + new Pose2d(new Translation2d(186.855, 195.662), new Rotation2d(Math.toRadians(120))), + new Pose2d(new Translation2d(182.245, 161.62), new Rotation2d(0)), + new Pose2d(new Translation2d(193.875, 123.75), new Rotation2d(Math.toRadians(240))), + new Pose2d(new Translation2d(78.375, 165), new Rotation2d(0)), + new Pose2d(new Translation2d(330, 165), new Rotation2d(0)), //Switch angle by 1pi if you want opposite rotation + new Pose2d(new Translation2d(585.75, 165), new Rotation2d(0)) //StageCenterRight }; // Poses are in the same order as the enumerator. // TODO: populate poses