diff --git a/.vscode/launch.json b/.vscode/launch.json index c9c9713..4dbe5d3 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,7 +4,6 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - { "type": "wpilib", "name": "WPILib Desktop Debug", diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5581adf..67cb117 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -42,15 +42,14 @@ 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; // ofsets are in degrees - //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; - public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0; + // 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; + public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0; // swerve PID constants public static final int SWERVE_SLOT_IDX = 0; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 93cffc9..4b437f1 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -18,7 +18,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; -import frc4388.utility.RobotGyro; public class SwerveDrive extends SubsystemBase { SwerveDriveKinematics m_kinematics; @@ -39,27 +38,15 @@ public class SwerveDrive extends SubsystemBase { public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; - Translation2d m_frontLeftLocation = - new Translation2d( - Units.inchesToMeters(halfHeight), - Units.inchesToMeters(halfWidth)); - 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)); - //setSwerveGains(); + Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(halfWidth)); + 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)); + // setSwerveGains(); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); public SwerveModule[] modules; - public RobotGyro gyro; //TODO Add Gyro Lol + public Gyro gyro; //TODO Add Gyro Lol public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor, diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index e9df20a..4176e52 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -90,4 +90,4 @@ public class SwerveModule extends SubsystemBase { double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER); } -} \ No newline at end of file +} diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java.old similarity index 100% rename from src/main/java/frc4388/utility/RobotGyro.java rename to src/main/java/frc4388/utility/RobotGyro.java.old