diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e0b59e1..41c5603 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.controller.ProfiledPIDController; 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.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import frc4388.utility.Gains; @@ -39,8 +40,18 @@ public final class Constants { public static final class SwerveDriveConstants { public static final double ROTATION_SPEED = 2.3; - public static final double WIDTH = 23.75; - public static final double HEIGHT = 23.75; + /** Distance between centers of right and left wheels on robot */ + public static final double TRACK_WIDTH = Units.inchesToMeters(23.75); + /** Distance between centers of front and back wheels on robot */ + public static final double WHEEL_BASE = Units.inchesToMeters(23.75); + + public static final SwerveDriveKinematics DRIVE_KINEMATICS = new SwerveDriveKinematics( + new Translation2d(WHEEL_BASE / 2, TRACK_WIDTH / 2), + new Translation2d(WHEEL_BASE / 2, -TRACK_WIDTH / 2), + new Translation2d(-WHEEL_BASE / 2, TRACK_WIDTH / 2), + new Translation2d(-WHEEL_BASE / 2, -TRACK_WIDTH / 2) + ); + public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2; public static final double MAX_SPEED_FEET_PER_SEC = 20; // TODO: redundant constant? @@ -55,10 +66,10 @@ public final class Constants { public static final int LEFT_BACK_WHEEL_CAN_ID = 7; public static final int RIGHT_BACK_STEER_CAN_ID = 8; public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; - public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; - public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; - public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; - public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; + public static final int LEFT_FRONT_STEER_ENCODER_CAN_ID = 10; + public static final int RIGHT_FRONT_STEER_ENCODER_CAN_ID = 11; + public static final int LEFT_BACK_STEER_ENCODER_CAN_ID = 12; + public static final int RIGHT_BACK_STEER_ENCODER_CAN_ID = 13; public static final int GYRO_ID = 14; // offsets are in degrees @@ -268,8 +279,8 @@ public final class Constants { public static final int SHOOTER_TIMEOUT_MS = 32; public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; - public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER = new SupplyCurrentLimitConfiguration( - true, 10, 0, 0); + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER = + new SupplyCurrentLimitConfiguration(true, 10, 0, 0); public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_SHOOTER = new StatorCurrentLimitConfiguration( true, 27, 0, 0); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 519f33b..8a5b8af 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -40,10 +40,10 @@ public class RobotMap { public final WPI_TalonFX backLeftDriveMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); public final WPI_TalonFX backRightSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); public final WPI_TalonFX backRightDriveMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); - public final CANCoder frontLeftEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); - public final CANCoder frontRightEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); - public final CANCoder backLeftEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); - public final CANCoder backRightEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); + public final CANCoder frontLeftEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_ENCODER_CAN_ID); + public final CANCoder frontRightEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_ENCODER_CAN_ID); + public final CANCoder backLeftEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_ENCODER_CAN_ID); + public final CANCoder backRightEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_ENCODER_CAN_ID); public SwerveModule frontLeft; public SwerveModule frontRight; diff --git a/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java index 84f66d1..b469dae 100644 --- a/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java +++ b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java @@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ClimberConstants; import frc4388.robot.subsystems.Claws; import frc4388.robot.subsystems.Climber; -import frc4388.utility.Vector2D; +import frc4388.utility.Vector2dExt; public class RunClimberPath extends CommandBase { Climber climber; diff --git a/src/main/java/frc4388/robot/commands/PathPlannerCommand.java b/src/main/java/frc4388/robot/commands/PathPlannerCommand.java index ea2a687..5e214b9 100644 --- a/src/main/java/frc4388/robot/commands/PathPlannerCommand.java +++ b/src/main/java/frc4388/robot/commands/PathPlannerCommand.java @@ -49,7 +49,7 @@ public class PathPlannerCommand extends SequentialCommandGroup { addCommands( new InstantCommand(() -> m_swerveDrive.resetOdometry(initialPosition), m_swerveDrive), - new PPSwerveControllerCommand(pathTrajectory, m_swerveDrive::getOdometry, m_swerveDrive.m_kinematics, xController, yController, thetaController, m_swerveDrive::setModuleStates, m_swerveDrive), + new PPSwerveControllerCommand(pathTrajectory, m_swerveDrive::getOdometry, SwerveDriveConstants.DRIVE_KINEMATICS, xController, yController, thetaController, m_swerveDrive::setModuleStates, m_swerveDrive), new InstantCommand(m_swerveDrive::stopModules, m_swerveDrive) ); } else { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 795268d..66feb7b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -6,8 +6,6 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.sensors.WPI_Pigeon2; -import edu.wpi.first.math.VecBuilder; -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; @@ -21,7 +19,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.utility.Gains; public class SwerveDrive extends SubsystemBase { @@ -30,19 +27,6 @@ public class SwerveDrive extends SubsystemBase { private SwerveModule m_backLeft; private SwerveModule m_backRight; - double halfWidth = SwerveDriveConstants.WIDTH / 2.d; - double halfHeight = SwerveDriveConstants.HEIGHT / 2.d; - - public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; - - Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfWidth), Units.inchesToMeters(-halfHeight)); - Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfWidth), Units.inchesToMeters(halfHeight)); - Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfWidth), Units.inchesToMeters(-halfHeight)); - Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfWidth), Units.inchesToMeters(halfHeight)); - - public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, - m_backLeftLocation, m_backRightLocation); - public SwerveModule[] modules; public WPI_Pigeon2 m_gyro; @@ -51,7 +35,7 @@ public class SwerveDrive extends SubsystemBase { public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public boolean ignoreAngles; - public Rotation2d rotTarget = new Rotation2d(); + public double rotTarget; private ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); public final Field2d m_field = new Field2d(); @@ -73,7 +57,7 @@ public class SwerveDrive extends SubsystemBase { // VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune // VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune - m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d()); + m_odometry = new SwerveDriveOdometry(SwerveDriveConstants.DRIVE_KINEMATICS, m_gyro.getRotation2d()); m_gyro.reset(); SmartDashboard.putData("Field", m_field); @@ -106,7 +90,7 @@ public class SwerveDrive extends SubsystemBase { -rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2))) : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond, -rot * SwerveDriveConstants.ROTATION_SPEED * 2); - SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds); + SwerveModuleState[] states = SwerveDriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } @@ -117,23 +101,16 @@ public class SwerveDrive extends SubsystemBase { } // new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) - public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { ignoreAngles = leftStick.getX() == 0 && leftStick.getY() == 0 && rightStick.getX() == 0 && rightStick.getY() == 0; leftStick = leftStick.times(leftStick.getNorm() * speedAdjust); if (Math.abs(rightStick.getX()) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightStick.getY()) > OIConstants.RIGHT_AXIS_DEADBAND) - rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0,1)); - double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); - if (ignoreAngles) { - rot = 0; - } - double xSpeedMetersPerSecond = leftStick.getX(); - double ySpeedMetersPerSecond = leftStick.getY(); + rotTarget = -(Math.atan2(rightStick.getY(), rightStick.getX()) - Math.PI / 2); + double rot = ignoreAngles ? rotTarget - m_gyro.getRotation2d().getRadians() : 0; chassisSpeeds = fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2))) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED * 2); - SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( - chassisSpeeds); + ? ChassisSpeeds.fromFieldRelativeSpeeds(leftStick.getX(), leftStick.getY(), rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI * 2) + (Math.PI / 2))) + : new ChassisSpeeds(leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED * 2); + SwerveModuleState[] states = SwerveDriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds); // if (ignoreAngles) { // SwerveModuleState[] lockedStates = new SwerveModuleState[states.length]; // for (int i = 0; i < states.length; i ++) { @@ -143,8 +120,6 @@ public class SwerveDrive extends SubsystemBase { // setModuleStates(lockedStates); // } setModuleStates(states); - // SmartDashboard.putNumber("rot", rot); - // SmartDashboard.putNumber("rotarget", rotTarget.getDegrees()); } /** @@ -266,7 +241,7 @@ public class SwerveDrive extends SubsystemBase { */ public void resetGyro() { m_gyro.reset(); - rotTarget = new Rotation2d(0); + rotTarget = 0; } /** diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2dExt.java similarity index 72% rename from src/main/java/frc4388/utility/Vector2D.java rename to src/main/java/frc4388/utility/Vector2dExt.java index 56bfe47..9a329a8 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2dExt.java @@ -14,25 +14,19 @@ import edu.wpi.first.wpilibj.drive.Vector2d; /** Aarav's good vector class (better than WPILib) * @author Aarav Shah */ -public class Vector2D extends Vector2d { - - public double x; - public double y; - public double angle; - - public Vector2D() { +public class Vector2dExt extends Vector2d { + public Vector2dExt() { this(0, 0); } - public Vector2D(double x, double y) { + public Vector2dExt(double x, double y) { super(x, y); this.x = x; this.y = y; - this.angle = Math.atan2(this.y, this.x); } - public Vector2D(double[] vec) { + public Vector2dExt(double[] vec) { this(vec[0], vec[1]); if (vec.length != 2) { @@ -40,11 +34,11 @@ public class Vector2D extends Vector2d { } } - public Vector2D(Point p) { + public Vector2dExt(Point p) { this(p.x, p.y); } - public Vector2D(Translation2d t) { + public Vector2dExt(Translation2d t) { this(t.getX(), t.getY()); } @@ -54,15 +48,15 @@ public class Vector2D extends Vector2d { * @param v2 Second vector in the addition. * @return New vector which is the sum. */ - public static Vector2D add(Vector2D v1, Vector2D v2) { - return new Vector2D(v1.x + v2.x, v1.y + v2.y); + public static Vector2dExt add(Vector2dExt v1, Vector2dExt v2) { + return new Vector2dExt(v1.x + v2.x, v1.y + v2.y); } /** * Adds vector to current object * @param v Vector to add */ - public void add(Vector2D v) { + public void add(Vector2dExt v) { this.x += v.x; this.y += v.x; } @@ -73,15 +67,15 @@ public class Vector2D extends Vector2d { * @param v2 Second vector in the subtraction. * @return New vector which is the difference. */ - public static Vector2D subtract(Vector2D v1, Vector2D v2) { - return new Vector2D(v1.x - v2.x, v1.y - v2.y); + public static Vector2dExt subtract(Vector2dExt v1, Vector2dExt v2) { + return new Vector2dExt(v1.x - v2.x, v1.y - v2.y); } /** * Subtracts vector from current object * @param v Vector to subtract */ - public void subtract(Vector2D v) { + public void subtract(Vector2dExt v) { this.x -= v.x; this.y -= v.x; } @@ -92,8 +86,8 @@ public class Vector2D extends Vector2d { * @param v2 Scalar to multiply. * @return New vector which is the product. */ - public static Vector2D multiply(Vector2D v1, double scalar) { - return new Vector2D(scalar * v1.x, scalar * v1.y); + public static Vector2dExt multiply(Vector2dExt v1, double scalar) { + return new Vector2dExt(scalar * v1.x, scalar * v1.y); } /** @@ -111,8 +105,8 @@ public class Vector2D extends Vector2d { * @param v2 Scalar to divide. * @return New vector which is the division. */ - public static Vector2D divide(Vector2D v1, double scalar) { - return new Vector2D(v1.x / scalar, v1.y / scalar); + public static Vector2dExt divide(Vector2dExt v1, double scalar) { + return new Vector2dExt(v1.x / scalar, v1.y / scalar); } /** @@ -128,8 +122,8 @@ public class Vector2D extends Vector2d { * Find unit vector. * @return The unit vector. */ - public Vector2D unit() { - return new Vector2D(this.x / this.magnitude(), this.y / this.magnitude()); + public Vector2dExt unit() { + return new Vector2dExt(this.x / this.magnitude(), this.y / this.magnitude()); } /** @@ -138,10 +132,10 @@ public class Vector2D extends Vector2d { * @param places Number of places to round to. * @return New rounded vector. */ - public static Vector2D round(Vector2D v, int places) { + public static Vector2dExt round(Vector2dExt v, int places) { int scale = (int) Math.pow(10, places); - v = Vector2D.multiply(v, scale); + v = Vector2dExt.multiply(v, scale); v.x = Math.round(v.x); v.y = Math.round(v.y); diff --git a/src/main/java/frc4388/utility/VelocityCorrection.java b/src/main/java/frc4388/utility/VelocityCorrection.java index 147657c..0cf49ca 100644 --- a/src/main/java/frc4388/utility/VelocityCorrection.java +++ b/src/main/java/frc4388/utility/VelocityCorrection.java @@ -14,29 +14,29 @@ public class VelocityCorrection { BoomBoom boomBoom; // vectors (in ft and ft/sec) - public Vector2D position; - public Vector2D cartesianVelocity; + public Vector2dExt position; + public Vector2dExt cartesianVelocity; // find - public Vector2D target; + public Vector2dExt target; public VelocityCorrection(SwerveDrive swerve, BoomBoom boomBoom) { this.swerve = swerve; this.boomBoom = boomBoom; - position = new Vector2D(5, 0);//new Vector2D(this.swerve.getOdometry().getX(), this.swerve.getOdometry().getY()); - cartesianVelocity = new Vector2D(-2, 3);//new Vector2D(this.swerve.getChassisSpeeds().vxMetersPerSecond, this.swerve.getChassisSpeeds().vyMetersPerSecond); + position = new Vector2dExt(5, 0);//new Vector2D(this.swerve.getOdometry().getX(), this.swerve.getOdometry().getY()); + cartesianVelocity = new Vector2dExt(-2, 3);//new Vector2D(this.swerve.getChassisSpeeds().vxMetersPerSecond, this.swerve.getChassisSpeeds().vyMetersPerSecond); target = getTargetPoint(); } - private Vector2D getTargetPoint() { + private Vector2dExt getTargetPoint() { double approxShotTime = 1; // TODO: get shot time from shooter tables - Vector2D targetPoint = Vector2D.multiply(this.cartesianVelocity, -1 * approxShotTime); + Vector2dExt targetPoint = Vector2dExt.multiply(this.cartesianVelocity, -1 * approxShotTime); - return Vector2D.round(targetPoint, 5); + return Vector2dExt.round(targetPoint, 5); } }