diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3b2a1da..4fdc273 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,70 +14,72 @@ public final class Constants { -public static final class DriveConstants { - // TODO: set motor and encoder constants - public static final int kFrontLeftDriveMotorPort = 32; - public static final int kRearLeftDriveMotorPort = 29; - public static final int kFrontRightDriveMotorPort = 38; - public static final int kRearRightDriveMotorPort = 34; - - public static final int kFrontLeftTurningMotorPort = 28; - public static final int kRearLeftTurningMotorPort = 22; - public static final int kFrontRightTurningMotorPort = 37; - public static final int kRearRightTurningMotorPort = 26; - - public static final int kFrontLeftTurningEncoderPort = 19; - public static final int kRearLeftTurningEncoderPort = 20; - public static final int kFrontRightTurningEncoderPort = 18; - public static final int kRearRightTurningEncoderPort = 17; - - public static final double kFrontLeftTurningEncoderOffset = 0; - public static final double kRearLeftTurningEncoderOffset = 0; - public static final double kFrontRightTurningEncoderOffset = 0; - public static final double kRearRightTurningEncoderOffset = 0; - - // TODO: Test motor orientations before driving on an actual robot - public static final boolean kFrontLeftDriveMotorReversed = false; - public static final boolean kRearLeftDriveMotorReversed = false; - public static final boolean kFrontRightDriveMotorReversed = true; - public static final boolean kRearRightDriveMotorReversed = true; - - /** Distance between centers of right and left wheels on robot (in meters). */ - public static final double kTrackWidth = 0.57785; - - /** Distance between front and back wheels on robot (in meters). */ - public static final double kWheelBase = 0.57785; - - /** Diameter of each wheel in the SDS MK4i swerve module (in meters) */ - public static final double kWheelDiameterMeters = 0.1; - - /** Gear ratio between the motor and the wheel. */ - public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration - // public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 configuration - - // TODO: Tune this PID before running on a robot on the ground - public static final double kPModuleTurningController = -0.3; - - public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2, kTrackWidth / 2), - new Translation2d(kWheelBase / 2, -kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); - - /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ - public static final double kMaxSpeedMetersPerSecond = 4.4196; - /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ - public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164; - // ^^ Calculated using the method taken from the old SDS github example - - /** Heading Correction */ - public static final double kHeadingCorrectionTurningStopTime = 0.2; - // TODO: Tune this PID before running on a robot on the ground - public static final double kPHeadingCorrectionController = 5; - } -//Intake PID and Encoder Constants + public static final class DriveConstants { + // TODO: set motor and encoder constants + public static final int kFrontLeftDriveMotorPort = 32; + public static final int kRearLeftDriveMotorPort = 29; + public static final int kFrontRightDriveMotorPort = 38; + public static final int kRearRightDriveMotorPort = 34; + + public static final int kFrontLeftTurningMotorPort = 28; + public static final int kRearLeftTurningMotorPort = 22; + public static final int kFrontRightTurningMotorPort = 37; + public static final int kRearRightTurningMotorPort = 26; + + public static final int kFrontLeftTurningEncoderPort = 19; + public static final int kRearLeftTurningEncoderPort = 20; + public static final int kFrontRightTurningEncoderPort = 18; + public static final int kRearRightTurningEncoderPort = 17; + + public static final double kFrontLeftTurningEncoderOffset = 0; + public static final double kRearLeftTurningEncoderOffset = 0; + public static final double kFrontRightTurningEncoderOffset = 0; + public static final double kRearRightTurningEncoderOffset = 0; + + // TODO: Test motor orientations before driving on an actual robot + public static final boolean kFrontLeftDriveMotorReversed = false; + public static final boolean kRearLeftDriveMotorReversed = false; + public static final boolean kFrontRightDriveMotorReversed = true; + public static final boolean kRearRightDriveMotorReversed = true; + + /** Distance between centers of right and left wheels on robot (in meters). */ + public static final double kTrackWidth = 0.57785; + + /** Distance between front and back wheels on robot (in meters). */ + public static final double kWheelBase = 0.57785; + + /** Diameter of each wheel in the SDS MK4i swerve module (in meters) */ + public static final double kWheelDiameterMeters = 0.1; + + /** Gear ratio between the motor and the wheel. */ + public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration + // public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 + // configuration + + // TODO: Tune this PID before running on a robot on the ground + public static final double kPModuleTurningController = -0.3; + + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + + /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ + public static final double kMaxSpeedMetersPerSecond = 4.4196; + /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ + public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164; + // ^^ Calculated using the method taken from the old SDS github example + + /** Heading Correction */ + public static final double kHeadingCorrectionTurningStopTime = 0.2; + // TODO: Tune this PID before running on a robot on the ground + public static final double kPHeadingCorrectionController = 5; + } + + // Intake PID and Encoder Constants public static class IntakeConstants { - public static final double kIntakeDroppedAngle = 9.0; + public static final double kIntakeLoweredAngle = 9.0; public static final double kIntakeRaisedAngle = 9.0; public static final int kIntakeMotorID = 0; public static final int kArmMotorID = 0; @@ -88,38 +90,35 @@ public static class IntakeConstants { public static final double kArmI = 0; public static final double kArmD = 0; public static final int kArmEncoderCh = 0; - public static final double IntakeDroppedAngle = 0; - public static final double IntakeRaisedAngle = 0; + public static double kIntakeSpeed; } - //Shooter subsystem speed constants + // Shooter subsystem speed constants public static class ShooterConstants { public static final double kSpinSpeedTrue = 0.75; public static final double kSpinSpeedFalse = 0; public static int kBottomShooterMotorPort; public static int kTopShooterMotorPort; + } + public static final class VisionConstants { + // TODO: Update cam pose relative to center of bot + public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); + public static final double[] kLimelightCamPose = { + kCamPose.getX(), + kCamPose.getY(), + kCamPose.getZ(), + kCamPose.getRotation().getX(), + kCamPose.getRotation().getY(), + kCamPose.getRotation().getZ() }; + + // TODO: Experiment with different std devs in the pose estimator + public static final Vector kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1); + public static final Vector kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9); + + // Field size in meters + public static final double kFieldWidth = 8.21055; + public static final double kFieldLength = 16.54175; } - - public static final class VisionConstants { - // TODO: Update cam pose relative to center of bot - public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); - public static final double[] kLimelightCamPose = { - kCamPose.getX(), - kCamPose.getY(), - kCamPose.getZ(), - kCamPose.getRotation().getX(), - kCamPose.getRotation().getY(), - kCamPose.getRotation().getZ() }; - - // TODO: Experiment with different std devs in the pose estimator - public static final Vector kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1); - public static final Vector kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9); - - // Field size in meters - public static final double kFieldWidth = 8.21055; - public static final double kFieldLength = 16.54175; - } } - diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 72c7663..3fe6a58 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,19 +29,18 @@ public class RobotContainer { private boolean IntakeDropped = false; private boolean lastAButton = false; - private XboxController controller = new XboxController(0); + private XboxController m_controller = new XboxController(0); public RobotContainer() { // Configure the trigger bindings configureBindings(); - m_intakeSubsystem.setDefaultCommand(new InstantCommand( - () -> m_intakeSubsystem.load(controller.getRightTriggerAxis() - controller.getLeftTriggerAxis(), - IntakeDropped ? IntakeConstants.IntakeDroppedAngle : IntakeConstants.IntakeRaisedAngle), - m_intakeSubsystem)); + m_intakeSubsystem.setDefaultCommand(m_controller.getLeftTriggerAxis() > 0.5 + ? new InstantCommand(m_intakeSubsystem::intakeDisk, m_intakeSubsystem) + : new InstantCommand(m_intakeSubsystem::stopIntaking, m_intakeSubsystem)); } public void periodic() { - if (controller.getAButton()) { + if (m_controller.getAButton()) { lastAButton = true; if (!lastAButton) IntakeDropped = !IntakeDropped; @@ -51,10 +50,16 @@ public void periodic() { } private void configureBindings() { - new JoystickButton(controller, Button.kB.value) + new JoystickButton(m_controller, Button.kY.value) + .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeLoweredAngle), m_intakeSubsystem)) + .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem)); + new JoystickButton(m_controller, Button.kX.value) + .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeRaisedAngle), m_intakeSubsystem)) + .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem)); + new JoystickButton(m_controller, Button.kB.value) .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem)) .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); - new JoystickButton(controller, Button.kA.value) + new JoystickButton(m_controller, Button.kA.value) .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem)) .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 794a666..ba7c052 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -9,7 +9,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.DutyCycleEncoder; - +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeConstants; @@ -17,25 +17,25 @@ public class IntakeSubsystem extends SubsystemBase { CANSparkFlex intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless); CANSparkFlex armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless); - - PIDController intakeVeloPID = new PIDController(IntakeConstants.kIntakeP,IntakeConstants.kIntakeI,IntakeConstants.kIntakeD); - PIDController armPID = new PIDController(IntakeConstants.kArmP,IntakeConstants.kArmI,IntakeConstants.kArmD); + + PIDController intakeVeloPID = new PIDController(IntakeConstants.kIntakeP, IntakeConstants.kIntakeI, + IntakeConstants.kIntakeD); + PIDController armPID = new PIDController(IntakeConstants.kArmP, IntakeConstants.kArmI, IntakeConstants.kArmD); DutyCycleEncoder armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderCh); - + /** Creates a new intake. */ public IntakeSubsystem() { - + } - /** - * - * @param speed motor power to apply to intake - *@param angle in radians - * - */ - public void load(double speed, double angle){ - intakeMotor.set(speed); - tiltToAngle(angle); + // Starts intaking the disk + public void intakeDisk() { + intakeMotor.set(IntakeConstants.kIntakeSpeed); + } + + // + public void stopIntaking() { + intakeMotor.set(0); } /** @@ -47,7 +47,10 @@ public void tiltToAngle(double angle) { double motorPower = armPID.calculate(armEncoder.getAbsolutePosition(), angle); armMotor.set(motorPower); } - + public void stopRotating(){ + + } + @Override public void periodic() { // This method will be called once per scheduler run