From d1a4a50f87b185b7e557dbd5fc18e843986e0e86 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 24 Jan 2022 19:06:28 -0700 Subject: [PATCH] changed the entire thing --- .../java/frc4388/robot/RobotContainer.java | 11 +-- src/main/java/frc4388/robot/RobotMap.java | 11 +++ .../frc4388/robot/subsystems/SwerveDrive.java | 82 ++++++++++--------- 3 files changed, 57 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9fbbd5b..393576d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -32,16 +32,7 @@ public class RobotContainer { /* Subsystems */ private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( - m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor, - m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor, - m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor, - m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor, - m_robotMap.leftFrontEncoder, - m_robotMap.rightFrontEncoder, - m_robotMap.leftBackEncoder, - m_robotMap.rightBackEncoder, - m_robotMap.gyro - ); + m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); private final LED m_robotLED = new LED(m_robotMap.LEDController); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index b740d43..fd84fef 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.subsystems.SwerveModule; /** * Defines and holds all I/O objects on the Roborio. This is useful for unit @@ -47,6 +48,11 @@ public class RobotMap { public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID); + public SwerveModule leftFront; + public SwerveModule leftBack; + public SwerveModule rightFront; + public SwerveModule rightBack; + void configureSwerveMotorControllers() { leftFrontSteerMotor.configFactoryDefault(); @@ -94,6 +100,11 @@ public class RobotMap { rightBackSteerMotor.setNeutralMode(NeutralMode.Brake); rightBackWheelMotor.setNeutralMode(NeutralMode.Coast); + leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET); + leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); + rightFront = new SwerveModule(rightFrontWheelMotor, rightFrontSteerMotor, rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); + rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); + // config cancoder as remote encoder for swerve steer motors //leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); //leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 6bbc89d..fafc14c 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -23,20 +23,27 @@ import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; public class SwerveDrive extends SubsystemBase { - 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; - private CANCoder m_leftFrontEncoder; - private CANCoder m_rightFrontEncoder; - private CANCoder m_leftBackEncoder; - private CANCoder m_rightBackEncoder; + // 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; + // private CANCoder m_leftFrontEncoder; + // private CANCoder m_rightFrontEncoder; + // private CANCoder m_leftBackEncoder; + // private CANCoder m_rightBackEncoder; + + private SwerveModule m_leftFront; + private SwerveModule m_leftBack; + private SwerveModule m_rightFront; + private SwerveModule m_rightBack; + double halfWidth = SwerveDriveConstants.WIDTH / 2.d; double halfHeight = SwerveDriveConstants.HEIGHT / 2.d; + public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; @@ -60,33 +67,34 @@ public class SwerveDrive extends SubsystemBase { public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public boolean ignoreAngles; - 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, CANCoder leftFrontEncoder, - CANCoder rightFrontEncoder, - CANCoder leftBackEncoder, - CANCoder rightBackEncoder, WPI_PigeonIMU gyro) - { - m_leftFrontSteerMotor = leftFrontSteerMotor; - m_leftFrontWheelMotor = leftFrontWheelMotor; - m_rightFrontSteerMotor = rightFrontSteerMotor; - m_rightFrontWheelMotor = rightFrontWheelMotor; - m_leftBackSteerMotor = leftBackSteerMotor; - m_leftBackWheelMotor = leftBackWheelMotor; - m_rightBackSteerMotor = rightBackSteerMotor; - m_rightBackWheelMotor = rightBackWheelMotor; - m_leftFrontEncoder = leftFrontEncoder; - m_rightFrontEncoder = rightFrontEncoder; - m_leftBackEncoder = leftBackEncoder; - m_rightBackEncoder = rightBackEncoder; - m_gyro = gyro; + public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, WPI_PigeonIMU gyro) { + // m_leftFrontSteerMotor = leftFrontSteerMotor; + // m_leftFrontWheelMotor = leftFrontWheelMotor; + // m_rightFrontSteerMotor = rightFrontSteerMotor; + // m_rightFrontWheelMotor = rightFrontWheelMotor; + // m_leftBackSteerMotor = leftBackSteerMotor; + // m_leftBackWheelMotor = leftBackWheelMotor; + // m_rightBackSteerMotor = rightBackSteerMotor; + // m_rightBackWheelMotor = rightBackWheelMotor; + // m_leftFrontEncoder = leftFrontEncoder; + // m_rightFrontEncoder = rightFrontEncoder; + // m_leftBackEncoder = leftBackEncoder; + // m_rightBackEncoder = rightBackEncoder; + m_leftFront = leftFront; + m_leftBack = leftBack; + m_rightFront = rightFront; + m_rightBack = rightBack; + 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 + // }; + + modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack}; - 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_poseEstimator = new SwerveDrivePoseEstimator( m_gyro.getRotation2d(),