mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-08 16:28:07 -06:00
Add some little swerve refactorings
Fix math issue with vector 2d extension
This commit is contained in:
nathanrsxtn
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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 {
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
+20
-26
@@ -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);
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user