diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 22137dd..931c600 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -33,8 +33,15 @@ public final class Constants { public static final double ROTATION_SPEED = 0.1; public static final double WIDTH = 0; public static final double HEIGHT = 0; + public static final int LEFT_FRONT_STEER_CAN_ID = 0; + public static final int LEFT_FRONT_WHEEL_CAN_ID = 0; + public static final int RIGHT_FRONT_STEER_CAN_ID = 0; + public static final int RIGHT_FRONT_WHEEL_CAN_ID = 0; + public static final int LEFT_BACK_STEER_CAN_ID = 0; + public static final int LEFT_BACK_WHEEL_CAN_ID = 0; + public static final int RIGHT_BACK_STEER_CAN_ID = 0; + public static final int RIGHT_BACK_WHEEL_CAN_ID = 0; } - public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index ac8d7d5..61eba53 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj.Spark; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import frc4388.robot.Constants.ArcadeDriveConstants; import frc4388.robot.Constants.LEDConstants; +import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.RobotGyro; /** @@ -68,4 +69,13 @@ public class RobotMap { leftBackMotor.setInverted(InvertType.FollowMaster); rightBackMotor.setInverted(InvertType.FollowMaster); } + /* Swerve Subsystem */ + public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); + public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); + public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); + public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); + public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); + public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); + public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); + public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 131cf44..448af23 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -7,6 +7,8 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; @@ -17,9 +19,25 @@ import frc4388.robot.Constants.SwerveDriveConstants; public class SwerveDrive { SwerveDriveKinematics m_kinematics; - - public SwerveDrive() + private WPI_TalonFX m_leftFrontSteerMotor; + private WPI_TalonFX m_leftFrontWheelMotor; + private WPI_TalonFX m_rightFrontSteerMotor; + private WPI_TalonFX m_rightFrontWheelMotor; + private WPI_TalonFX m_leftBackSteerMotor; + private WPI_TalonFX m_leftBackWheelMotor; + private WPI_TalonFX m_rightBackSteerMotor; + private WPI_TalonFX m_rightBackWheelMotor; + public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor, + WPI_TalonFX leftBackSteerMotor,WPI_TalonFX leftBackWheelMotor,WPI_TalonFX rightBackSteerMotor,WPI_TalonFX rightBackWheelMotor) { + m_leftFrontSteerMotor = leftFrontSteerMotor; + m_leftFrontWheelMotor = leftFrontWheelMotor; + m_rightFrontSteerMotor = rightFrontSteerMotor; + m_rightFrontWheelMotor = rightFrontWheelMotor; + m_leftBackSteerMotor = leftBackSteerMotor; + m_leftBackWheelMotor = leftBackWheelMotor; + m_rightBackSteerMotor = rightBackSteerMotor; + m_rightBackWheelMotor = rightBackWheelMotor; double halfWidth = SwerveDriveConstants.WIDTH / 2.d; double halfHeight = SwerveDriveConstants.HEIGHT / 2.d; @@ -49,6 +67,7 @@ public class SwerveDrive // Back right module state SwerveModuleState backRight = SwerveModuleState.optimize(moduleStates[3], new Rotation2d(/*get encoder positions*/)); + } // public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate)