diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h index be4490e9d6..0b486e0711 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h @@ -34,7 +34,7 @@ constexpr int kRearRightMotorPort = 3; constexpr int kFrontLeftEncoderPorts[]{0, 1}; constexpr int kRearLeftEncoderPorts[]{2, 3}; constexpr int kFrontRightEncoderPorts[]{4, 5}; -constexpr int kRearRightEncoderPorts[]{5, 6}; +constexpr int kRearRightEncoderPorts[]{6, 7}; constexpr bool kFrontLeftEncoderReversed = false; constexpr bool kRearLeftEncoderReversed = true; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h index 6c6db533d1..db02c729d5 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h @@ -36,8 +36,8 @@ class SwerveModule { frc::PWMSparkMax m_driveMotor; frc::PWMSparkMax m_turningMotor; - frc::Encoder m_driveEncoder{0, 1}; - frc::Encoder m_turningEncoder{2, 3}; + frc::Encoder m_driveEncoder; + frc::Encoder m_turningEncoder; frc2::PIDController m_drivePIDController{1.0, 0, 0}; frc::ProfiledPIDController m_turningPIDController{ diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h index efb40ed302..d41eeda548 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h @@ -39,17 +39,17 @@ constexpr int kRearRightTurningMotorPort = 7; constexpr int kFrontLeftTurningEncoderPorts[2]{0, 1}; constexpr int kRearLeftTurningEncoderPorts[2]{2, 3}; constexpr int kFrontRightTurningEncoderPorts[2]{4, 5}; -constexpr int kRearRightTurningEncoderPorts[2]{5, 6}; +constexpr int kRearRightTurningEncoderPorts[2]{6, 7}; constexpr bool kFrontLeftTurningEncoderReversed = false; constexpr bool kRearLeftTurningEncoderReversed = true; constexpr bool kFrontRightTurningEncoderReversed = false; constexpr bool kRearRightTurningEncoderReversed = true; -constexpr int kFrontLeftDriveEncoderPorts[2]{0, 1}; -constexpr int kRearLeftDriveEncoderPorts[2]{2, 3}; -constexpr int kFrontRightDriveEncoderPorts[2]{4, 5}; -constexpr int kRearRightDriveEncoderPorts[2]{5, 6}; +constexpr int kFrontLeftDriveEncoderPorts[2]{8, 9}; +constexpr int kRearLeftDriveEncoderPorts[2]{10, 11}; +constexpr int kFrontRightDriveEncoderPorts[2]{12, 13}; +constexpr int kRearRightDriveEncoderPorts[2]{14, 15}; constexpr bool kFrontLeftDriveEncoderReversed = false; constexpr bool kRearLeftDriveEncoderReversed = true; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp index 6bb982f7fe..e7879ff658 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp @@ -8,8 +8,15 @@ #include SwerveModule::SwerveModule(const int driveMotorChannel, - const int turningMotorChannel) - : m_driveMotor(driveMotorChannel), m_turningMotor(turningMotorChannel) { + const int turningMotorChannel, + const int driveEncoderChannelA, + const int driveEncoderChannelB, + const int turningEncoderChannelA, + const int turningEncoderChannelB) + : m_driveMotor(driveMotorChannel), + m_turningMotor(turningMotorChannel), + m_driveEncoder(driveEncoderChannelA, driveEncoderChannelB), + m_turningEncoder(turningEncoderChannelA, turningEncoderChannelB) { // Set the distance per pulse for the drive encoder. We can simply use the // distance traveled for one rotation of the wheel divided by the encoder // resolution. diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h index 1f9d857178..c75002f130 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h @@ -35,10 +35,10 @@ class Drivetrain { frc::Translation2d m_backLeftLocation{-0.381_m, +0.381_m}; frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m}; - SwerveModule m_frontLeft{1, 2}; - SwerveModule m_frontRight{2, 3}; - SwerveModule m_backLeft{5, 6}; - SwerveModule m_backRight{7, 8}; + SwerveModule m_frontLeft{1, 2, 0, 1, 2, 3}; + SwerveModule m_frontRight{2, 3, 4, 5, 6, 7}; + SwerveModule m_backLeft{5, 6, 8, 9, 10, 11}; + SwerveModule m_backRight{7, 8, 12, 13, 14, 15}; frc::AnalogGyro m_gyro{0}; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h index 0a33cb9dd4..8ce7adea58 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h @@ -18,7 +18,9 @@ class SwerveModule { public: - SwerveModule(int driveMotorChannel, int turningMotorChannel); + SwerveModule(int driveMotorChannel, int turningMotorChannel, + int driveEncoderChannelA, int driveEncoderChannelB, + int turningEncoderChannelA, int turningEncoderChannelB); frc::SwerveModuleState GetState() const; void SetDesiredState(const frc::SwerveModuleState& state); @@ -34,8 +36,8 @@ class SwerveModule { frc::PWMSparkMax m_driveMotor; frc::PWMSparkMax m_turningMotor; - frc::Encoder m_driveEncoder{0, 1}; - frc::Encoder m_turningEncoder{2, 3}; + frc::Encoder m_driveEncoder; + frc::Encoder m_turningEncoder; frc2::PIDController m_drivePIDController{1.0, 0, 0}; frc::ProfiledPIDController m_turningPIDController{ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java index 895983d272..41cf8c571e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java @@ -27,7 +27,7 @@ public final class Constants { public static final int[] kFrontLeftEncoderPorts = new int[] {0, 1}; public static final int[] kRearLeftEncoderPorts = new int[] {2, 3}; public static final int[] kFrontRightEncoderPorts = new int[] {4, 5}; - public static final int[] kRearRightEncoderPorts = new int[] {5, 6}; + public static final int[] kRearRightEncoderPorts = new int[] {6, 7}; public static final boolean kFrontLeftEncoderReversed = false; public static final boolean kRearLeftEncoderReversed = true; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java index a2df728252..129369d0a2 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java @@ -55,7 +55,8 @@ public class RobotContainer { m_driverController.getY(GenericHID.Hand.kLeft), m_driverController.getX(GenericHID.Hand.kRight), m_driverController.getX(GenericHID.Hand.kLeft), - false))); + false), + m_robotDrive)); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java index 40a352237c..a47ece8c4a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java @@ -31,17 +31,17 @@ public final class Constants { public static final int[] kFrontLeftTurningEncoderPorts = new int[] {0, 1}; public static final int[] kRearLeftTurningEncoderPorts = new int[] {2, 3}; public static final int[] kFrontRightTurningEncoderPorts = new int[] {4, 5}; - public static final int[] kRearRightTurningEncoderPorts = new int[] {5, 6}; + public static final int[] kRearRightTurningEncoderPorts = new int[] {6, 7}; public static final boolean kFrontLeftTurningEncoderReversed = false; public static final boolean kRearLeftTurningEncoderReversed = true; public static final boolean kFrontRightTurningEncoderReversed = false; public static final boolean kRearRightTurningEncoderReversed = true; - public static final int[] kFrontLeftDriveEncoderPorts = new int[] {7, 8}; - public static final int[] kRearLeftDriveEncoderPorts = new int[] {9, 10}; - public static final int[] kFrontRightDriveEncoderPorts = new int[] {11, 12}; - public static final int[] kRearRightDriveEncoderPorts = new int[] {13, 14}; + public static final int[] kFrontLeftDriveEncoderPorts = new int[] {8, 9}; + public static final int[] kRearLeftDriveEncoderPorts = new int[] {10, 11}; + public static final int[] kFrontRightDriveEncoderPorts = new int[] {12, 13}; + public static final int[] kRearRightDriveEncoderPorts = new int[] {14, 15}; public static final boolean kFrontLeftDriveEncoderReversed = false; public static final boolean kRearLeftDriveEncoderReversed = true; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java index 3a8f903dc0..1455b4b56b 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java @@ -53,7 +53,8 @@ public class RobotContainer { m_driverController.getY(GenericHID.Hand.kLeft), m_driverController.getX(GenericHID.Hand.kRight), m_driverController.getX(GenericHID.Hand.kLeft), - false))); + false), + m_robotDrive)); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Drivetrain.java index dd6843be8a..8c5aebdb04 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/Drivetrain.java @@ -24,10 +24,10 @@ public class Drivetrain { private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381); private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381); - private final SwerveModule m_frontLeft = new SwerveModule(1, 2); - private final SwerveModule m_frontRight = new SwerveModule(3, 4); - private final SwerveModule m_backLeft = new SwerveModule(5, 6); - private final SwerveModule m_backRight = new SwerveModule(7, 8); + private final SwerveModule m_frontLeft = new SwerveModule(1, 2, 0, 1, 2, 3); + private final SwerveModule m_frontRight = new SwerveModule(3, 4, 4, 5, 6, 7); + private final SwerveModule m_backLeft = new SwerveModule(5, 6, 8, 9, 10, 11); + private final SwerveModule m_backRight = new SwerveModule(7, 8, 12, 13, 14, 15); private final AnalogGyro m_gyro = new AnalogGyro(0); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/SwerveModule.java index 3110bc2040..1cfb62061c 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/SwerveModule.java @@ -25,11 +25,13 @@ public class SwerveModule { private final MotorController m_driveMotor; private final MotorController m_turningMotor; - private final Encoder m_driveEncoder = new Encoder(0, 1); - private final Encoder m_turningEncoder = new Encoder(2, 3); + private final Encoder m_driveEncoder; + private final Encoder m_turningEncoder; + // Gains are for example purposes only - must be determined for your own robot! private final PIDController m_drivePIDController = new PIDController(1, 0, 0); + // Gains are for example purposes only - must be determined for your own robot! private final ProfiledPIDController m_turningPIDController = new ProfiledPIDController( 1, @@ -43,15 +45,28 @@ public class SwerveModule { private final SimpleMotorFeedforward m_turnFeedforward = new SimpleMotorFeedforward(1, 0.5); /** - * Constructs a SwerveModule. + * Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning encoder. * - * @param driveMotorChannel ID for the drive motor. - * @param turningMotorChannel ID for the turning motor. + * @param driveMotorChannel PWM output for the drive motor. + * @param turningMotorChannel PWM output for the turning motor. + * @param driveEncoderChannelA DIO input for the drive encoder channel A + * @param driveEncoderChannelB DIO input for the drive encoder channel B + * @param turningEncoderChannelA DIO input for the turning encoder channel A + * @param turningEncoderChannelB DIO input for the turning encoder channel B */ - public SwerveModule(int driveMotorChannel, int turningMotorChannel) { + public SwerveModule( + int driveMotorChannel, + int turningMotorChannel, + int driveEncoderChannelA, + int driveEncoderChannelB, + int turningEncoderChannelA, + int turningEncoderChannelB) { m_driveMotor = new PWMSparkMax(driveMotorChannel); m_turningMotor = new PWMSparkMax(turningMotorChannel); + m_driveEncoder = new Encoder(driveEncoderChannelA, driveEncoderChannelB); + m_turningEncoder = new Encoder(turningEncoderChannelA, turningEncoderChannelB); + // Set the distance per pulse for the drive encoder. We can simply use the // distance traveled for one rotation of the wheel divided by the encoder // resolution.