diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 931c600..d87bad9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -41,6 +41,10 @@ public final class Constants { 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 int LEFT_FRONT_STEER_CAN_ENCODER_ID = 0; + public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 0; + public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 0; + public static final int RIGHT_BACK_STEER_CAN_ENCODER_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 61eba53..1f2f139 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -11,6 +11,7 @@ import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.PigeonIMU; +import com.ctre.phoenix.sensors.CANCoder; import edu.wpi.first.wpilibj.Spark; import edu.wpi.first.wpilibj.drive.DifferentialDrive; @@ -78,4 +79,11 @@ public class RobotMap { 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); + public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); + public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); + public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); + public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); + + void configureSwerveMotorController() { + } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 448af23..0896ec2 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -8,6 +8,7 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.CANCoder; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; @@ -27,8 +28,15 @@ public class SwerveDrive private WPI_TalonFX m_leftBackWheelMotor; private WPI_TalonFX m_rightBackSteerMotor; private WPI_TalonFX m_rightBackWheelMotor; + private CANCoder m_leftFrontEncoder; + private CANCoder m_rightFrontEncoder; + private CANCoder m_leftBackEncoder; + private CANCoder m_rightBackEncoder; 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) + WPI_TalonFX leftBackSteerMotor,WPI_TalonFX leftBackWheelMotor,WPI_TalonFX rightBackSteerMotor,WPI_TalonFX rightBackWheelMotor, CANCoder leftFrontEncoder, + CANCoder rightFrontEncoder, + CANCoder leftBackEncoder, + CANCoder rightBackEncoder) { m_leftFrontSteerMotor = leftFrontSteerMotor; m_leftFrontWheelMotor = leftFrontWheelMotor; @@ -38,6 +46,10 @@ public class SwerveDrive m_leftBackWheelMotor = leftBackWheelMotor; m_rightBackSteerMotor = rightBackSteerMotor; m_rightBackWheelMotor = rightBackWheelMotor; + m_leftFrontEncoder = leftFrontEncoder; + m_rightFrontEncoder = rightFrontEncoder; + m_leftBackEncoder = leftBackEncoder; + m_rightBackEncoder = rightBackEncoder; double halfWidth = SwerveDriveConstants.WIDTH / 2.d; double halfHeight = SwerveDriveConstants.HEIGHT / 2.d; @@ -57,16 +69,16 @@ public class SwerveDrive SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds); // Front left module state - SwerveModuleState frontLeft = SwerveModuleState.optimize(moduleStates[0], new Rotation2d(/*get encoder positions*/)); + SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], new Rotation2d(m_leftFrontEncoder.getPosition())); // Front right module state - SwerveModuleState frontRight = SwerveModuleState.optimize(moduleStates[1], new Rotation2d(/*get encoder positions*/)); + SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], new Rotation2d(m_rightFrontEncoder.getPosition())); // Back left module state - SwerveModuleState backLeft = SwerveModuleState.optimize(moduleStates[2], new Rotation2d(/*get encoder positions*/)); + SwerveModuleState lefBack = SwerveModuleState.optimize(moduleStates[2], new Rotation2d(m_leftBackEncoder.getPosition())); // Back right module state - SwerveModuleState backRight = SwerveModuleState.optimize(moduleStates[3], new Rotation2d(/*get encoder positions*/)); + SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], new Rotation2d(m_rightBackEncoder.getPosition())); }