From bfa044e340a7f9261800b87887207855d3741936 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 20 Jan 2022 19:00:42 -0700 Subject: [PATCH] constants reworked (needs testing, which will be done with odometry testing) --- src/main/java/frc4388/robot/Constants.java | 25 ++++++++++++++----- .../frc4388/robot/subsystems/SwerveDrive.java | 25 ++++++++++++------- .../robot/subsystems/SwerveModule.java | 2 +- 3 files changed, 36 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 817d46d..fdaaec2 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -30,7 +30,8 @@ public final class Constants { public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2; public static final double MAX_SPEED_FEET_PER_SEC = 20; public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; - //IDs + + //IDs public static final int LEFT_FRONT_STEER_CAN_ID = 2; public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; public static final int RIGHT_FRONT_STEER_CAN_ID = 4; @@ -44,11 +45,8 @@ public final class Constants { 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 GYRO_ID = 14; + // ofsets are in degrees - // public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141; - // public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281; - // public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531; - // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594; public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0; public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0; public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0; @@ -66,7 +64,22 @@ public final class Constants { public static final int REMOTE_0 = 0; // conversions - // 5.12 rev motor = 1 rev wheel + // gear ratio: 5.12 rev motor = 1 rev wheel + // wheel diameter: official = 4 in, measured = 3.8 in + /* Ratio Calculation */ + public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; + public static final double WHEEL_DIAMETER_INCHES = 3.8; + public static final double TICKS_PER_MOTOR_REV = 2048; + public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; + public static final double INCHES_PER_METER = 39.370; + public static final double METERS_PER_INCH = 1/INCHES_PER_METER; + + public static final double WHEEL_REV_PER_MOTOR_REV = 1/MOTOR_REV_PER_WHEEL_REV; + public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; + public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV/INCHES_PER_WHEEL_REV; + public static final double INCHES_PER_TICK = 1/TICKS_PER_INCH; + public static final double TICK_TIME_TO_SECONDS = 0.1; + public static final double SECONDS_TO_TICK_TIME = 1/TICK_TIME_TO_SECONDS; // misc public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 61fe8f1..1940592 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -22,6 +22,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -52,6 +53,12 @@ public class SwerveDrive extends SubsystemBase { Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth)); Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth)); Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth)); + + SwerveModule m_frontLeft = new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET); + SwerveModule m_frontRight = new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); + SwerveModule m_backLeft = new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); + SwerveModule m_backRight = new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); + // setSwerveGains(); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); @@ -94,10 +101,10 @@ public class SwerveDrive extends SubsystemBase { m_gyro = gyro; modules = new SwerveModule[] { - new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left - new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET), // Front Right - new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left - new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET) // Back Right + m_frontLeft, // Front Left + m_frontRight, // Front Right + m_backLeft, // Back Left + m_backRight // Back Right }; m_gyro.reset(); } @@ -144,17 +151,17 @@ public class SwerveDrive extends SubsystemBase { /** Updates the field relative position of the robot. */ public void updateOdometry() { m_poseEstimator.update( m_gyro.getRotation2d(), - m_frontLeftLocation.getState(), - m_frontRightLocation.getState(), - m_backLeftLocation.getState(), - m_backRightLocation.getState()); + m_frontLeft.getState(), + m_frontRight.getState(), + m_backLeft.getState(), + m_backRight.getState()); // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on // a real robot, this must be calculated based either on latency or timestamps. m_poseEstimator.addVisionMeasurement( ExampleGlobalMeasurementSensor.getEstimatedGlobalPose( m_poseEstimator.getEstimatedPosition()), - Timer.getFPGATimestamp() - 0.3); + Timer.getFPGATimestamp() - 0.1); } public void highSpeed(boolean shift){ diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 20aaa4b..3b2b192 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -100,7 +100,7 @@ public class SwerveModule extends SubsystemBase { * @return The current state of the module. */ public SwerveModuleState getState() { - return new SwerveModuleState(driveMotor.getSelectedSensorVelocity(), new Rotation2d(canCoder.getPosition())); + return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK * SwerveDriveConstants.METERS_PER_INCH * 10, new Rotation2d(canCoder.getPosition())); } }