diff --git a/2022NoWayHome.code-workspace b/2022NoWayHome.code-workspace index 077020e..a210c54 100644 --- a/2022NoWayHome.code-workspace +++ b/2022NoWayHome.code-workspace @@ -2,9 +2,6 @@ "folders": [ { "path": "." - }, - { - "path": "NetworkTablesDesktopClient" } ], "settings": { diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9f49a96..2b40696 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -40,10 +40,10 @@ public final class Constants { public static final double WHEEL_BASE = Units.inchesToMeters(23.75); public static final SwerveDriveKinematics DRIVE_KINEMATICS = new SwerveDriveKinematics( - new Translation2d(WHEEL_BASE / 2, TRACK_WIDTH / 2), new Translation2d(WHEEL_BASE / 2, -TRACK_WIDTH / 2), - new Translation2d(-WHEEL_BASE / 2, TRACK_WIDTH / 2), - new Translation2d(-WHEEL_BASE / 2, -TRACK_WIDTH / 2) + new Translation2d(WHEEL_BASE / 2, TRACK_WIDTH / 2), + new Translation2d(-WHEEL_BASE / 2, -TRACK_WIDTH / 2), + new Translation2d(-WHEEL_BASE / 2, TRACK_WIDTH / 2) ); public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d62126d..818a6f0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -123,7 +123,7 @@ public class RobotContainer { m_robotIntake.setDefaultCommand(new RunCommand(() -> m_robotIntake.runWithTriggers(getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake DefaultCommand")); // Shooter Idle - m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom).withName("BoomBoom DefaultCommand")); + // m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom).withName("BoomBoom DefaultCommand")); // Serializer Manual m_robotSerializer.setDefaultCommand(new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8), m_robotSerializer).withName("Serializer DefaultCommand")); @@ -340,13 +340,13 @@ public class RobotContainer { builder.addBooleanProperty("Shooter Safety", this::isLockedOn, null); }); putData("Field", m_robotSwerveDrive.m_field); - putData("PDP", new PowerDistribution() { - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.setSmartDashboardType("PowerDistributionPanel"); - } - }); + // putData("PDP", new PowerDistribution() { + // @Override + // public void initSendable(SendableBuilder builder) { + // super.initSendable(builder); + // builder.setSmartDashboardType("PowerDistributionPanel"); + // } + // }); putData("Extender", new NTSendable() { @Override public void initSendable(NTSendableBuilder builder) { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index d86dd7c..4f63806 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.utility.Gains; public class SwerveDrive extends SubsystemBase { @@ -44,7 +45,7 @@ public class SwerveDrive extends SubsystemBase { m_gyro = gyro; modules = new SwerveModule[] { m_frontLeft, m_frontRight, m_backLeft, m_backRight }; - + SmartDashboard.putNumber("OMEGA", 0.0); // m_poseEstimator = new SwerveDrivePoseEstimator( // getRegGyro(),//m_gyro.getRotation2d(), // new Pose2d(), @@ -90,7 +91,7 @@ public class SwerveDrive extends SubsystemBase { if (rightStick.getNorm() > OIConstants.RIGHT_AXIS_DEADBAND) rotTarget = -Math.atan2(rightStick.getY(), rightStick.getX()); double rotDifference = rotTarget - m_gyro.getRotation2d().getRadians(); leftStick = leftStick.times(leftStick.getNorm() * speedAdjust); - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(leftStick.getX(), leftStick.getY(), rotDifference * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI * 2) + (Math.PI / 2))); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(leftStick.getX(), leftStick.getY(), rotDifference, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI * 2) + (Math.PI / 2))); } else chassisSpeeds = new ChassisSpeeds(leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED * 2); setModuleStates(SwerveDriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds)); }