From 385948c03fe6a49ee90ddd0e3048e5559d25e859 Mon Sep 17 00:00:00 2001 From: BJ Gerling <107522124+vkskoll@users.noreply.github.com> Date: Tue, 6 Feb 2024 18:51:53 -0600 Subject: [PATCH 1/3] updated to match 4011 2024 robot --- build.gradle | 2 +- src/main/java/frc/robot/Constants.java | 30 ++++---- src/main/java/frc/robot/RobotContainer.java | 1 + src/main/java/frc/robot/SwerveModule.java | 6 +- .../java/frc/robot/subsystems/Swerve.java | 2 +- vendordeps/REVLib.json | 74 +++++++++++++++++++ 6 files changed, 95 insertions(+), 20 deletions(-) create mode 100644 vendordeps/REVLib.json diff --git a/build.gradle b/build.gradle index c73a804be..2348bae42 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" } java { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 897d7f8cf..6789c70be 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -15,15 +15,15 @@ public final class Constants { public static final double stickDeadband = 0.1; - public static final class Swerve { - public static final int pigeonID = 1; + public static final class Swerve { + public static final int pigeonID = 13; public static final COTSTalonFXSwerveConstants chosenModule = //TODO: This must be tuned to specific robot - COTSTalonFXSwerveConstants.SDS.MK4i.Falcon500(COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L2); + COTSTalonFXSwerveConstants.SDS.MK4i.Falcon500(COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L3); /* Drivetrain Constants */ - public static final double trackWidth = Units.inchesToMeters(21.73); //TODO: This must be tuned to specific robot - public static final double wheelBase = Units.inchesToMeters(21.73); //TODO: This must be tuned to specific robot + public static final double trackWidth = Units.inchesToMeters(23.81); //TODO: This must be tuned to specific robot + public static final double wheelBase = Units.inchesToMeters(23.63); //TODO: This must be tuned to specific robot public static final double wheelCircumference = chosenModule.wheelCircumference; /* Swerve Kinematics @@ -92,8 +92,8 @@ public static final class Swerve { public static final class Mod0 { //TODO: This must be tuned to specific robot public static final int driveMotorID = 1; public static final int angleMotorID = 2; - public static final int canCoderID = 1; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(0.0); + public static final int canCoderID = 9; + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(101.953); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -102,18 +102,18 @@ public static final class Mod0 { //TODO: This must be tuned to specific robot public static final class Mod1 { //TODO: This must be tuned to specific robot public static final int driveMotorID = 3; public static final int angleMotorID = 4; - public static final int canCoderID = 2; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(0.0); + public static final int canCoderID = 10; + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(68.379); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } /* Back Left Module - Module 2 */ public static final class Mod2 { //TODO: This must be tuned to specific robot - public static final int driveMotorID = 5; + public static final int driveMotorID = 5 ; public static final int angleMotorID = 6; - public static final int canCoderID = 3; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(0.0); + public static final int canCoderID = 11; + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(173.936); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -122,8 +122,8 @@ public static final class Mod2 { //TODO: This must be tuned to specific robot public static final class Mod3 { //TODO: This must be tuned to specific robot public static final int driveMotorID = 7; public static final int angleMotorID = 8; - public static final int canCoderID = 4; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(0.0); + public static final int canCoderID = 12; + public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-3.604); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -131,7 +131,7 @@ public static final class Mod3 { //TODO: This must be tuned to specific robot public static final class AutoConstants { //TODO: The below constants are used in the example auto, and must be tuned to specific robot public static final double kMaxSpeedMetersPerSecond = 3; - public static final double kMaxAccelerationMetersPerSecondSquared = 3; + public static final double kMaxAccelerationMetersPerSecondSquared =3; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8365d9467..38c55deb5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ public class RobotContainer { /* Controllers */ private final Joystick driver = new Joystick(0); + //private final XboxController driver = new XboxController(0); /* Drive Controls */ private final int translationAxis = XboxController.Axis.kLeftY.value; diff --git a/src/main/java/frc/robot/SwerveModule.java b/src/main/java/frc/robot/SwerveModule.java index 30991bc9f..869e7c384 100644 --- a/src/main/java/frc/robot/SwerveModule.java +++ b/src/main/java/frc/robot/SwerveModule.java @@ -35,16 +35,16 @@ public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants){ this.angleOffset = moduleConstants.angleOffset; /* Angle Encoder Config */ - angleEncoder = new CANcoder(moduleConstants.cancoderID); + angleEncoder = new CANcoder(moduleConstants.cancoderID, "canivore"); angleEncoder.getConfigurator().apply(Robot.ctreConfigs.swerveCANcoderConfig); /* Angle Motor Config */ - mAngleMotor = new TalonFX(moduleConstants.angleMotorID); + mAngleMotor = new TalonFX(moduleConstants.angleMotorID, "canivore"); mAngleMotor.getConfigurator().apply(Robot.ctreConfigs.swerveAngleFXConfig); resetToAbsolute(); /* Drive Motor Config */ - mDriveMotor = new TalonFX(moduleConstants.driveMotorID); + mDriveMotor = new TalonFX(moduleConstants.driveMotorID, "canivore"); mDriveMotor.getConfigurator().apply(Robot.ctreConfigs.swerveDriveFXConfig); mDriveMotor.getConfigurator().setPosition(0.0); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 4d245065b..5e707942e 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -24,7 +24,7 @@ public class Swerve extends SubsystemBase { public Pigeon2 gyro; public Swerve() { - gyro = new Pigeon2(Constants.Swerve.pigeonID); + gyro = new Pigeon2(Constants.Swerve.pigeonID, "canivore"); gyro.getConfigurator().apply(new Pigeon2Configuration()); gyro.setYaw(0); diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 000000000..0f3520e7f --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.0", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.2.0", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From 3f126773bedddf5977c59a6c4f6e481ffc75da22 Mon Sep 17 00:00:00 2001 From: BJ Gerling <107522124+vkskoll@users.noreply.github.com> Date: Tue, 6 Feb 2024 18:52:16 -0600 Subject: [PATCH 2/3] updated team number --- .wpilib/wpilib_preferences.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 02708d607..8a62166b2 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2024", - "teamNumber": 364 + "teamNumber": 4011 } \ No newline at end of file From 5cee1cb55a2ab7ed6dec485ebf514e55467f1b74 Mon Sep 17 00:00:00 2001 From: BJ Gerling <107522124+vkskoll@users.noreply.github.com> Date: Fri, 9 Feb 2024 15:32:23 -0600 Subject: [PATCH 3/3] 2024 robot spec + base shooter elements --- src/main/java/frc/robot/Constants.java | 5 ++ src/main/java/frc/robot/RobotContainer.java | 52 ++++++++++++--- .../frc/robot/commands/KickerCommand.java | 45 +++++++++++++ .../frc/robot/commands/ShooterCommand.java | 49 ++++++++++++++ .../frc/robot/subsystems/KickerSubsystem.java | 34 ++++++++++ .../robot/subsystems/ShooterSubsystem.java | 66 +++++++++++++++++++ 6 files changed, 243 insertions(+), 8 deletions(-) create mode 100644 src/main/java/frc/robot/commands/KickerCommand.java create mode 100644 src/main/java/frc/robot/commands/ShooterCommand.java create mode 100644 src/main/java/frc/robot/subsystems/KickerSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/ShooterSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6789c70be..4ee027cca 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -18,6 +18,11 @@ public final class Constants { public static final class Swerve { public static final int pigeonID = 13; + public static final class ShooterConstants { + public static final double kRightShooterCMDVolts = 10.0; + public static final double kLeftShooterCMDVolts = 6.0; + } + public static final COTSTalonFXSwerveConstants chosenModule = //TODO: This must be tuned to specific robot COTSTalonFXSwerveConstants.SDS.MK4i.Falcon500(COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L3); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 38c55deb5..057c45847 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,7 +6,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; - +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.Swerve.ShooterConstants; import frc.robot.autos.*; import frc.robot.commands.*; import frc.robot.subsystems.*; @@ -19,8 +20,12 @@ */ public class RobotContainer { /* Controllers */ - private final Joystick driver = new Joystick(0); - //private final XboxController driver = new XboxController(0); + // private final Joystick driver = new Joystick(0); + + private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); + private final KickerSubsystem kickerSubsystem = new KickerSubsystem(); + private final XboxController driverXbox = new XboxController(0); + /* Drive Controls */ private final int translationAxis = XboxController.Axis.kLeftY.value; @@ -28,8 +33,9 @@ public class RobotContainer { private final int rotationAxis = XboxController.Axis.kRightX.value; /* Driver Buttons */ - private final JoystickButton zeroGyro = new JoystickButton(driver, XboxController.Button.kY.value); - private final JoystickButton robotCentric = new JoystickButton(driver, XboxController.Button.kLeftBumper.value); + private final JoystickButton zeroGyro = new JoystickButton(driverXbox, XboxController.Button.kY.value); + private final JoystickButton robotCentric = new JoystickButton(driverXbox, XboxController.Button.kLeftBumper.value); + /* Subsystems */ private final Swerve s_Swerve = new Swerve(); @@ -40,12 +46,14 @@ public RobotContainer() { s_Swerve.setDefaultCommand( new TeleopSwerve( s_Swerve, - () -> -driver.getRawAxis(translationAxis), - () -> -driver.getRawAxis(strafeAxis), - () -> -driver.getRawAxis(rotationAxis), + () -> -driverXbox.getRawAxis(translationAxis), + () -> -driverXbox.getRawAxis(strafeAxis), + () -> -driverXbox.getRawAxis(rotationAxis), () -> robotCentric.getAsBoolean() ) ); + + shooterSubsystem.setDefaultCommand(new ShooterCommand(shooterSubsystem, 0,0)); // Configure the button bindings configureButtonBindings(); @@ -60,8 +68,36 @@ public RobotContainer() { private void configureButtonBindings() { /* Driver Buttons */ zeroGyro.onTrue(new InstantCommand(() -> s_Swerve.zeroHeading())); + + //new Trigger(driverXbox.getLeftTriggerAxis() > 0.5) .whileTrue(new ShooterCommand(shooterSubsytem, 10.0, 6.0 )); + + // driverXbox.a().whileTrue(new ShooterCommand(shooterSubsytem , 10.0,6.0)); + //new JoystickButton(driverXbox, 1).whileTrue(new ShooterCommand(shooterSubsytem, 10.0, 6.0)); + + + new JoystickButton(driverXbox, 1).whileTrue(new ShooterCommand(shooterSubsystem, 6.0, 10.0 )); + new JoystickButton(driverXbox, 2).whileTrue(new KickerCommand(kickerSubsystem, 11.0)); + /*if(new JoystickButton(driverXbox, 1).whileTrue(new ShooterCommand(shooterSubsystem, 6.0, 10.0)) != null) { + if(shooterSubsystem.getRightMotorVoltage() <= 10.0){ + new KickerCommand(kickerSubsystem, 11.0); + + } + }*/ + } + + /*public void ampShooter() { + if(driverXbox.getAButtonPressed()) { + new ShooterCommand(shooterSubsystem, 6.0, 10.0); + if(shooterSubsystem.getRightMotorVoltage() >= 10.0 ){ + new KickerCommand(kickerSubsystem, 5.0); + } + + + + } + }*/ /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/commands/KickerCommand.java b/src/main/java/frc/robot/commands/KickerCommand.java new file mode 100644 index 000000000..0bd5b56ec --- /dev/null +++ b/src/main/java/frc/robot/commands/KickerCommand.java @@ -0,0 +1,45 @@ +// 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.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.KickerSubsystem; + +public class KickerCommand extends Command { + /** Creates a new KickerCommand. */ + private final KickerSubsystem kickerSubsystem; + private double kickerVoltage; + public KickerCommand(KickerSubsystem kickerSubsystem, double kickerVoltage) { + // Use addRequirements() here to declare subsystem dependencies. + this.kickerSubsystem = kickerSubsystem; + this.kickerVoltage = kickerVoltage; + addRequirements(kickerSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + System.out.println("kickerCMD started"); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + kickerSubsystem.setKickerSpeed( kickerVoltage); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + kickerSubsystem.setKickerSpeed(0); + System.out.println("kickerCMD stopped"); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommand.java new file mode 100644 index 000000000..bd80cde50 --- /dev/null +++ b/src/main/java/frc/robot/commands/ShooterCommand.java @@ -0,0 +1,49 @@ +// 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.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ShooterSubsystem; + +public class ShooterCommand extends Command { + /** Creates a new ShooterCommand. */ + private final ShooterSubsystem shooterSubsytem; + private double leftVoltage; + private double rightVoltage; + // private double kickerVoltage; + public ShooterCommand(ShooterSubsystem shooterSubsytem, double leftVoltage, double rightVoltage) { + // Use addRequirements() here to declare subsystem dependencies. + this.shooterSubsytem = shooterSubsytem; + this.leftVoltage = leftVoltage; + this.rightVoltage = rightVoltage; + //this.kickerVoltage = kickerVoltage; + addRequirements(shooterSubsytem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + System.out.println("ShooterCMD started"); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + shooterSubsytem.setShooterSpeed(leftVoltage, rightVoltage); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + shooterSubsytem.setShooterSpeed(0, 0); + System.out.println("ShooterCMD stopped"); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/KickerSubsystem.java b/src/main/java/frc/robot/subsystems/KickerSubsystem.java new file mode 100644 index 000000000..fd75fa9d4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/KickerSubsystem.java @@ -0,0 +1,34 @@ +// 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.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class KickerSubsystem extends SubsystemBase { + /** Creates a new KickerSubsystem. */ + private static CANSparkMax kickerMotor; + public KickerSubsystem() { + // kickerMotor.restoreFactoryDefaults(); + kickerMotor = new CANSparkMax(14, MotorType.kBrushless); + kickerMotor.restoreFactoryDefaults(); + kickerMotor.setInverted(false); + kickerMotor.setIdleMode(IdleMode.kBrake); + kickerMotor.setSmartCurrentLimit(80); + kickerMotor.burnFlash(); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + public void setKickerSpeed(double kickerVoltage) { + kickerMotor.setVoltage(kickerVoltage); + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 000000000..2de02c7bf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,66 @@ +// 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.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ShooterSubsystem extends SubsystemBase { + /** Creates a new ShooterSubsytem. */ + private static CANSparkMax rightShooterMotor; + private static CANSparkMax leftShooterMotor; + //private static CANSparkMax kickerMotor; + public ShooterSubsystem() { + rightShooterMotor = new CANSparkMax(20, MotorType.kBrushless); + rightShooterMotor.restoreFactoryDefaults(); + rightShooterMotor.setInverted(true); + rightShooterMotor.setIdleMode(IdleMode.kCoast); + rightShooterMotor.setSmartCurrentLimit(80); + rightShooterMotor.burnFlash(); + + + leftShooterMotor = new CANSparkMax(18, MotorType.kBrushless); + leftShooterMotor.restoreFactoryDefaults(); + leftShooterMotor.setInverted(false); + leftShooterMotor.setIdleMode(IdleMode.kCoast); + leftShooterMotor.setSmartCurrentLimit(80); + leftShooterMotor.burnFlash(); + + /* kickerMotor.restoreFactoryDefaults(); + kickerMotor = new CANSparkMax(16, MotorType.kBrushless); + kickerMotor.setInverted(false); + kickerMotor.setIdleMode(IdleMode.kBrake); + kickerMotor.setSmartCurrentLimit(80); + kickerMotor.burnFlash();*/ + + + } + + @Override + public void periodic() { + + // This method will be called once per scheduler run + } + public void setShooterSpeed(double leftVoltage, double rightVoltage) { + leftShooterMotor.setVoltage(leftVoltage); + rightShooterMotor.setVoltage(rightVoltage); + } + /*public double getRightMotorVoltage(){ + return rightShooterMotor.getBusVoltage(); + }*/ + + public double getRightMotorVoltage() { + return rightShooterMotor.getVoltageCompensationNominalVoltage(); + } + + /*public void setKickerSpeed(double kickerVoltage) { + kickerMotor.setVoltage(kickerVoltage); + }*/ + +}