diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index aa4bfd6..a963b8d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,102 +25,121 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - - /** - * Input/Output constants - */ - public static final class IOConstants { - public static final int kDriverControllerPort = 0; - - public static final double kControllerDeadband = 0.2; - public static final double kSlowModeScalar = 0.8; - } - - 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; - } - - public static final class ShooterConstants { - public static final int kTopShooterMotorPort = 35; - public static final int kBottomShooterMotorPort = 20; - } - - 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; - } - + /** + * Input/Output constants + */ + public static final class IOConstants { + public static final int kDriverControllerPort = 0; + + public static final double kControllerDeadband = 0.2; + public static final double kSlowModeScalar = 0.8; + } + + 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 kIntakeLoweredAngle = 9.0; + public static final double kIntakeRaisedAngle = 9.0; + public static final int kIntakeMotorID = 0; + public static final int kArmMotorID = 0; + public static final double kIntakeP = 0; + public static final double kIntakeI = 0; + public static final double kIntakeD = 0; + public static final double kArmP = 0; + public static final double kArmI = 0; + public static final double kArmD = 0; + public static final int kArmEncoderCh = 0; + public static double kIntakeSpeed; + } + + // 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; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c19c224..535c73b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,9 +12,11 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.IOConstants; import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; /* @@ -24,18 +26,22 @@ * (including subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // The robot's subsystems and commands are defined here + private final DriveSubsystem m_robotDrive = new DriveSubsystem(); public final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); + public final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); + + private boolean IntakeDropped = false; + private boolean lastAButton = false; private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ public RobotContainer() { // Configure the trigger bindings configureBindings(); + m_intakeSubsystem.setDefaultCommand(m_driverController.getLeftTriggerAxis() > 0.5 + ? new InstantCommand(m_intakeSubsystem::intakeDisk, m_intakeSubsystem) + : new InstantCommand(m_intakeSubsystem::stopIntaking, m_intakeSubsystem)); m_robotDrive.setDefaultCommand( new RunCommand( @@ -66,16 +72,32 @@ public RobotContainer() { / 2, !m_driverController.getRightBumper()), m_robotDrive)); + + } + + public void periodic() { + if (m_driverController.getAButton()) { + lastAButton = true; + if (!lastAButton) + IntakeDropped = !IntakeDropped; + } else { + lastAButton = false; + } } /** * Use this method to define your button->command mappings. */ private void configureBindings() { - new JoystickButton(m_driverController, Button.kStart.value) - .onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive)); - // TODO: Move shoot commands to operator controller + new JoystickButton(m_driverController, Button.kY.value) + .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeLoweredAngle), + m_intakeSubsystem)) + .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem)); + new JoystickButton(m_driverController, Button.kX.value) + .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeRaisedAngle), + m_intakeSubsystem)) + .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem)); new JoystickButton(m_driverController, Button.kB.value) .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem)) .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); @@ -92,4 +114,4 @@ private void configureBindings() { public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..9d55b9c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,62 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants.IntakeConstants; + + +public class IntakeSubsystem extends SubsystemBase { + CANSparkFlex m_intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless); + CANSparkFlex m_armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless); + + PIDController m_intakeVeloPID = new PIDController(IntakeConstants.kIntakeP, IntakeConstants.kIntakeI, + IntakeConstants.kIntakeD); + PIDController m_armPID = new PIDController(IntakeConstants.kArmP, IntakeConstants.kArmI, IntakeConstants.kArmD); + + DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderCh); + + /** Creates a new intake. */ + public IntakeSubsystem() { + + } + + // Starts intaking the disk + public void intakeDisk() { + m_intakeMotor.set(IntakeConstants.kIntakeSpeed); + } + + //Stops rotating the intake + public void stopIntaking() { + m_intakeMotor.set(0); + } + + /** + * Rotates the arm to a given angle + * @param angle motor to apply to intake + * + */ + public void tiltToAngle(double angle) { + m_armMotor.set(m_armPID.calculate(m_armEncoder.getAbsolutePosition(), angle)); + } + + //Stops rotating the arm + public void stopRotating(){ + m_armMotor.set(0); + } + + @Override + public void periodic() { + SmartDashboard.putBoolean("Intake",m_intakeMotor.get()>0); + + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index afdbd19..a47de12 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -29,4 +29,4 @@ public void periodic() { // This method will be called once per scheduler run // SmartDashboard.putNumber("Speed", m_bottom.); } -} +} \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 69a4079..2b7d172 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.1.0", + "version": "24.2.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.1.0" + "version": "24.2.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true,