diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a53804b..9d3a05b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -33,20 +33,20 @@ public final class Constants { public static final class SwerveDriveConstants { public static final double ROTATION_SPEED = 0.1; public static final double WHEEL_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 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 double WIDTH = 22; + public static final double HEIGHT = 22; + public static final int LEFT_FRONT_STEER_CAN_ID = 2; + public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; + public static final int RIGHT_FRONT_STEER_CAN_ID = 4; + public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; + public static final int LEFT_BACK_STEER_CAN_ID = 6; + public static final int LEFT_BACK_WHEEL_CAN_ID = 7; + public static final int RIGHT_BACK_STEER_CAN_ID = 8; + public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; + public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; + public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; + 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 public static final float LEFT_FRONT_ENCODER_OFFSET = 0; public static final float RIGHT_FRONT_ENCODER_OFFSET = 0; @@ -57,7 +57,7 @@ public final class Constants { public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_PID_LOOP_IDX = 1; public static final int SWERVE_TIMEOUT_MS = 30; - public static final Gains SWERVE_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); // swerve configuration public static final double NEUTRAL_DEADBAND = 0.04; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d548108..b248748 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -32,8 +32,8 @@ public class RobotContainer { private final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final ArcadeDrive m_robotArcadeDrive = new ArcadeDrive(m_robotMap.leftFrontMotor, m_robotMap.rightFrontMotor, - m_robotMap.leftBackMotor, m_robotMap.rightBackMotor, m_robotMap.driveTrain, m_robotMap.gyroDrive); + //private final ArcadeDrive m_robotArcadeDrive = new ArcadeDrive(m_robotMap.leftFrontMotor, m_robotMap.rightFrontMotor, + // m_robotMap.leftBackMotor, m_robotMap.rightBackMotor, m_robotMap.driveTrain, m_robotMap.gyroDrive); private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor, @@ -82,8 +82,8 @@ public class RobotContainer { private void configureButtonBindings() { /* Driver Buttons */ // test command to spin the robot while pressing A on the driver controller - new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whileHeld(() -> m_robotArcadeDrive.driveWithInput(0, 1)); + //new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + //.whileHeld(() -> m_robotArcadeDrive.driveWithInput(0, 1)); /* Operator Buttons */ // activates "Lit Mode" diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 85da254..5b3ec1a 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -30,7 +30,7 @@ public class RobotMap { public RobotMap() { configureLEDMotorControllers(); - configureArcadeDriveMotorControllers(); + //configureArcadeDriveMotorControllers(); configureSwerveMotorControllers(); } @@ -40,39 +40,39 @@ public class RobotMap { void configureLEDMotorControllers() { } - + /* ArcadeDrive Subsystem */ - public final WPI_TalonFX leftFrontMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_LEFT_FRONT_CAN_ID); - public final WPI_TalonFX rightFrontMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); - public final WPI_TalonFX leftBackMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_LEFT_BACK_CAN_ID); - public final WPI_TalonFX rightBackMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_RIGHT_BACK_CAN_ID); - public final DifferentialDrive driveTrain = new DifferentialDrive(leftFrontMotor, rightFrontMotor); - public final RobotGyro gyroDrive = new RobotGyro(new PigeonIMU(ArcadeDriveConstants.DRIVE_PIGEON_ID)); + //public final WPI_TalonFX leftFrontMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_LEFT_FRONT_CAN_ID); + //public final WPI_TalonFX rightFrontMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); + //public final WPI_TalonFX leftBackMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_LEFT_BACK_CAN_ID); + //public final WPI_TalonFX rightBackMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_RIGHT_BACK_CAN_ID); + //public final DifferentialDrive driveTrain = new DifferentialDrive(leftFrontMotor, rightFrontMotor); + //public final RobotGyro gyroDrive = new RobotGyro(new PigeonIMU(ArcadeDriveConstants.DRIVE_PIGEON_ID)); - void configureArcadeDriveMotorControllers() { + /*void configureArcadeDriveMotorControllers() { - /* factory default values */ + // factory default values leftFrontMotor.configFactoryDefault(); rightFrontMotor.configFactoryDefault(); leftBackMotor.configFactoryDefault(); rightBackMotor.configFactoryDefault(); - /* set back motors as followers */ + // set back motors as followers leftBackMotor.follow(leftFrontMotor); rightBackMotor.follow(rightFrontMotor); - /* set neutral mode */ + // set neutral mode leftFrontMotor.setNeutralMode(NeutralMode.Brake); rightFrontMotor.setNeutralMode(NeutralMode.Brake); leftFrontMotor.setNeutralMode(NeutralMode.Brake); rightFrontMotor.setNeutralMode(NeutralMode.Brake); - /* flip input so forward becomes back, etc */ + // flip input so forward becomes back, etc leftFrontMotor.setInverted(false); rightFrontMotor.setInverted(false); 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); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 3a40438..ebe9d94 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.utility.Gains; public class SwerveDrive extends SubsystemBase { @@ -106,7 +107,8 @@ public class SwerveDrive extends SubsystemBase m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees()); m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees()); m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees()); - + } + public void setSwerveGains(){ m_leftFrontWheelMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); m_leftFrontWheelMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); @@ -117,7 +119,7 @@ public class SwerveDrive extends SubsystemBase m_rightFrontWheelMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); m_rightFrontWheelMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_rightFrontWheelMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_rightrFontWheelMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_rightFrontWheelMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_rightFrontWheelMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_leftBackWheelMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); @@ -132,8 +134,6 @@ public class SwerveDrive extends SubsystemBase m_rightBackWheelMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_rightBackWheelMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - } - // PID }