From fdcd1b5d5c964d681e94b3a4a0259edf16a1e37d Mon Sep 17 00:00:00 2001 From: robo7660 Date: Thu, 7 Mar 2024 17:12:57 -0500 Subject: [PATCH 01/13] fix advanced drive --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 15 ++++++++------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b89741d5..74e335b0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -107,7 +107,7 @@ public static class OperatorConstants { public static final int kDriverControllerPort = 0; public static final double LEFT_X_DEADBAND = 0.01; public static final double LEFT_Y_DEADBAND = 0.01; - public static final double RIGHT_X_DEADBAND = 0.01; + public static final double RIGHT_X_DEADBAND = 0.1; public static final int TURN_CONSTANT = 6; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f3d8d28f..f674e153 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,6 +31,7 @@ import frc.robot.subsystems.SwerveSubsystem; import frc.robot.subsystems.Transfer; import java.io.File; +import java.util.function.DoubleSupplier; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -71,12 +72,12 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); - int povAngle = driver.getPOV(); + DoubleSupplier povAngle = () -> driver.getPOV(); - Trigger driverUp = new Trigger(() -> (povAngle >= 315 || povAngle <= 45)); - Trigger driverDown = new Trigger(() -> (povAngle >= 135 && povAngle <= 225)); - Trigger driverLeft = new Trigger(() -> (povAngle >= 225 && povAngle <= 315)); - Trigger driverRight = new Trigger(() -> (povAngle >= 45 && povAngle <= 135)); + Trigger driverUp = new Trigger(() -> (povAngle.getAsDouble() >= 315 || (povAngle.getAsDouble() <= 45 && povAngle.getAsDouble() >= 0))); + Trigger driverDown = new Trigger(() -> (povAngle.getAsDouble() >= 135 && povAngle.getAsDouble() <= 225)); + Trigger driverLeft = new Trigger(() -> (povAngle.getAsDouble() >= 225 && povAngle.getAsDouble() <= 315)); + Trigger driverRight = new Trigger(() -> (povAngle.getAsDouble() >= 45 && povAngle.getAsDouble() <= 135)); AbsoluteDrive closedAbsoluteDrive = new AbsoluteDrive( @@ -94,13 +95,13 @@ public RobotContainer() { m_swerve, () -> MathUtil.applyDeadband(-driver.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), () -> MathUtil.applyDeadband(-driver.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> MathUtil.applyDeadband(-driver.getRightX(), OperatorConstants.RIGHT_X_DEADBAND), + () -> MathUtil.applyDeadband(driver.getRightX(), OperatorConstants.RIGHT_X_DEADBAND), () -> driverUp.getAsBoolean(), () -> driverDown.getAsBoolean(), () -> driverLeft.getAsBoolean(), () -> driverRight.getAsBoolean()); - m_swerve.setDefaultCommand(!RobotBase.isSimulation() ? closedAbsoluteDrive : advancedDrive); + m_swerve.setDefaultCommand(!RobotBase.isSimulation() ? advancedDrive : advancedDrive); // -m_index.setDefaultCommand(m_index.manualIntake(coDriver::getRightY)); From ac5f6170a83b72276035536fbb061fa8b52956ba Mon Sep 17 00:00:00 2001 From: robo7660 Date: Thu, 7 Mar 2024 17:13:06 -0500 Subject: [PATCH 02/13] remove unused printline --- src/main/java/frc/robot/subsystems/Index.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Index.java b/src/main/java/frc/robot/subsystems/Index.java index ee2c5064..41ed4321 100644 --- a/src/main/java/frc/robot/subsystems/Index.java +++ b/src/main/java/frc/robot/subsystems/Index.java @@ -111,7 +111,6 @@ public void periodic() { @Override public void simulationPeriodic() { // This method will be called once per scheduler run during simulation - System.out.println(isPrimed()); if (isPrimed() == true) { stop(); } From 1e1f9512347d825485b0a7edf4411ec8c3430ca5 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Thu, 14 Mar 2024 17:36:44 -0400 Subject: [PATCH 03/13] Change swerve offsets --- src/main/deploy/swerves/KrakenSwerve/modules/frontleft.json | 2 +- src/main/deploy/swerves/KrakenSwerve/modules/frontright.json | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/frontleft.json b/src/main/deploy/swerves/KrakenSwerve/modules/frontleft.json index 4a854b8b..a0b11ec8 100644 --- a/src/main/deploy/swerves/KrakenSwerve/modules/frontleft.json +++ b/src/main/deploy/swerves/KrakenSwerve/modules/frontleft.json @@ -18,7 +18,7 @@ "drive": true, "angle": false }, - "absoluteEncoderOffset": 135, + "absoluteEncoderOffset": 315, "location": { "front": 13, "left": 13 diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/frontright.json b/src/main/deploy/swerves/KrakenSwerve/modules/frontright.json index 542ac431..bf30a1e2 100644 --- a/src/main/deploy/swerves/KrakenSwerve/modules/frontright.json +++ b/src/main/deploy/swerves/KrakenSwerve/modules/frontright.json @@ -18,7 +18,7 @@ "drive": true, "angle": false }, - "absoluteEncoderOffset": 58, + "absoluteEncoderOffset": 105, "location": { "front": 13, "left": -13 From d7595f98552084b35fff1aea6effd7d65248fda0 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Thu, 14 Mar 2024 17:36:58 -0400 Subject: [PATCH 04/13] enable foc maybe --- .../frc/robot/subsystems/SwerveSubsystem.java | 34 ++++++++++++++++++- 1 file changed, 33 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index ebf051bb..b0b2a945 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -4,12 +4,14 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.hardware.core.CoreTalonFX; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -32,11 +34,15 @@ import frc.robot.Robot; import java.io.File; +import java.util.ArrayList; import java.util.function.DoubleSupplier; + import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.SwerveDriveTest; +import swervelib.SwerveModule; import swervelib.math.SwerveMath; +import swervelib.motors.TalonFXSwerve; import swervelib.parser.SwerveControllerConfiguration; import swervelib.parser.SwerveDriveConfiguration; import swervelib.parser.SwerveParser; @@ -49,6 +55,7 @@ public class SwerveSubsystem extends SubsystemBase * Swerve drive object. */ private final SwerveDrive swerveDrive; + private final ArrayList driveTalons = new ArrayList(); /** * Maximum speed of the robot in meters per second, used to limit acceleration. */ @@ -91,6 +98,8 @@ public SwerveSubsystem(File directory) setupPathPlanner(); setupCustom(); + initDriveTalons(); + setupFOC(); } /** @@ -104,6 +113,8 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig swerveDrive = new SwerveDrive(driveCfg, controllerCfg, maximumSpeed); setupCustom(); + initDriveTalons(); + setupFOC(); } /** @@ -627,6 +638,27 @@ private void populateDashboard() { SmartDashboard.putNumber("Rotation Set", 0); } + private void initDriveTalons() { + TalonFXSwerve sm; + CoreTalonFX c; + for (SwerveModule module : swerveDrive.getModules()){ + if(module.getDriveMotor() instanceof TalonFXSwerve){ + sm = (TalonFXSwerve) module.getDriveMotor(); + c = (CoreTalonFX) sm.getMotor(); + if(c instanceof TalonFX){ + driveTalons.add((TalonFX) c); + } + } + } + } + + private void setupFOC() { + DutyCycleOut control = new DutyCycleOut(1, true, false, false, false); + for(TalonFX talon : driveTalons){ + talon.setControl(control); + } + } + public Command dashboardPositionResetCommand() { return this.runOnce(() -> resetToDashboard()); } From ae1c132cb17ef53bde480d9e7304c724562c02d8 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Thu, 14 Mar 2024 17:37:25 -0400 Subject: [PATCH 05/13] Tune for new/old launcher --- src/main/java/frc/robot/Constants.java | 13 +++++++------ src/main/java/frc/robot/Robot.java | 3 --- src/main/java/frc/robot/subsystems/Launcher.java | 9 ++++----- 3 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 74e335b0..23d42b1d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -58,7 +58,7 @@ public static class Launch { public static final double launcherI = 0; public static final double launcherD = 0; public static final double launcherFF = 0.000156; - public static final double angleP = 0.00025; + public static final double angleP = 0.0045; public static final double angleI = 0; public static final double angleD = 0; public static final double angleFF = 0; @@ -67,12 +67,12 @@ public static class Launch { public static final int angleID = 46; public static final double allowedVeloPercent = 10; public static final double allowedDifferencePercent = 5; - public static final double closeLaunchPosition = 585; - public static final double farLaunchPosition = 655; - public static final double angleMin = 581; - public static final double angleMax = 660; + public static final double closeLaunchPosition = 330; + public static final double farLaunchPosition = 262; + public static final double angleMin = 261; + public static final double angleMax = 333; public static final boolean angleMotorInverted = true; - public static final boolean angleEncoderInverted = true; + public static final boolean angleEncoderInverted = false; public static final boolean lowerMotorInverted = true; public static final boolean upperMotorInverted = false; public static final double speedFarSpeaker = 2800; @@ -93,6 +93,7 @@ public static class Climb { public static final int winchLimitLeft = 2; public static final int winchLimitRight = 3; public static final double motorSpeedFactor = -0.9; + public static final double deadzone = 0.1; } public static class Transfer { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 99a6a76b..ee6c645f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -34,7 +34,6 @@ public void robotInit() { // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); getAlliance(); - m_robotContainer.lockLauncher(); } private void getAlliance() { @@ -78,7 +77,6 @@ public void autonomousInit() { } getAlliance(); - m_robotContainer.dropLauncher(); } /** This function is called periodically during autonomous. */ @@ -96,7 +94,6 @@ public void teleopInit() { } getAlliance(); m_robotContainer.updatePose(); - m_robotContainer.lockLauncher(); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/subsystems/Launcher.java b/src/main/java/frc/robot/subsystems/Launcher.java index b94ec3d0..e8948a2e 100644 --- a/src/main/java/frc/robot/subsystems/Launcher.java +++ b/src/main/java/frc/robot/subsystems/Launcher.java @@ -191,16 +191,15 @@ public void periodic() { updatePIDFromDashboard("Launcher", this::updateLauncherPIDs); updatePIDFromDashboard("Angle", this::updateAnglePIDs); } - angle.set(0); - /*if (goalPosition == LaunchPosition.FAR) { - angleController.setReference(Constants.Launch.farLaunchPosition, ControlType.kSmartMotion); + if (goalPosition == LaunchPosition.FAR) { + angleController.setReference(Constants.Launch.farLaunchPosition, ControlType.kPosition); curPosition = goalPosition; SmartDashboard.putNumber("Angle Desired", Constants.Launch.farLaunchPosition); } else { - angleController.setReference(Constants.Launch.closeLaunchPosition, ControlType.kSmartMotion); + angleController.setReference(Constants.Launch.closeLaunchPosition, ControlType.kPosition); curPosition = goalPosition; SmartDashboard.putNumber("Angle Desired", Constants.Launch.closeLaunchPosition); - }*/ + } SmartDashboard.putNumber("Angle Speed", angle.get()); SmartDashboard.putNumber("Launch Curr Velo", getCurrentVelocity()); SmartDashboard.putNumber("Angle Position", angleEncoder.getPosition()); From d94adb103c2070e4cbd7fd7fea800abfeed03591 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Thu, 14 Mar 2024 17:37:38 -0400 Subject: [PATCH 06/13] better POV buttons --- src/main/java/frc/robot/RobotContainer.java | 24 ++++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f674e153..99f3117c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,6 +23,7 @@ import frc.robot.commands.LaunchWithVelo; import frc.robot.commands.LaunchWithVeloAuton; import frc.robot.commands.PrimeIndex; +import frc.robot.commands.SwitchLaunchAngle; import frc.robot.commands.ToggleIntake; import frc.robot.subsystems.Climb; import frc.robot.subsystems.Index; @@ -72,12 +73,19 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); - DoubleSupplier povAngle = () -> driver.getPOV(); + DoubleSupplier povAngle = () -> driver.getPOV(); - Trigger driverUp = new Trigger(() -> (povAngle.getAsDouble() >= 315 || (povAngle.getAsDouble() <= 45 && povAngle.getAsDouble() >= 0))); - Trigger driverDown = new Trigger(() -> (povAngle.getAsDouble() >= 135 && povAngle.getAsDouble() <= 225)); - Trigger driverLeft = new Trigger(() -> (povAngle.getAsDouble() >= 225 && povAngle.getAsDouble() <= 315)); - Trigger driverRight = new Trigger(() -> (povAngle.getAsDouble() >= 45 && povAngle.getAsDouble() <= 135)); + Trigger driverUp = + new Trigger( + () -> + (povAngle.getAsDouble() >= 315 + || (povAngle.getAsDouble() <= 45 && povAngle.getAsDouble() >= 0))); + Trigger driverDown = + new Trigger(() -> (povAngle.getAsDouble() >= 135 && povAngle.getAsDouble() <= 225)); + Trigger driverLeft = + new Trigger(() -> (povAngle.getAsDouble() >= 225 && povAngle.getAsDouble() <= 315)); + Trigger driverRight = + new Trigger(() -> (povAngle.getAsDouble() >= 45 && povAngle.getAsDouble() <= 135)); AbsoluteDrive closedAbsoluteDrive = new AbsoluteDrive( @@ -101,11 +109,11 @@ public RobotContainer() { () -> driverLeft.getAsBoolean(), () -> driverRight.getAsBoolean()); - m_swerve.setDefaultCommand(!RobotBase.isSimulation() ? advancedDrive : advancedDrive); + m_swerve.setDefaultCommand(!RobotBase.isSimulation() ? closedAbsoluteDrive : advancedDrive); // -m_index.setDefaultCommand(m_index.manualIntake(coDriver::getRightY)); - m_climb.setDefaultCommand(m_climb.setWinchCommand(coDriver::getLeftY)); + m_climb.setDefaultCommand(m_climb.setWinchCommand(() -> MathUtil.applyDeadband(coDriver.getLeftY(), Constants.Climb.deadzone))); // add auto options m_chooser.setDefaultOption("Test Drive", m_swerve.getAutonomousCommand("Test Drive")); @@ -161,7 +169,7 @@ private void configureBindings() { a.whileTrue(new LaunchWithVelo(m_launch, m_index, 2050, false)); JoystickButton y = new JoystickButton(driver, XboxController.Button.kY.value); - y.whileTrue(m_intake.reverseIntakeCommand()); + y.whileTrue(new SwitchLaunchAngle(m_launch)); JoystickButton b = new JoystickButton(driver, XboxController.Button.kB.value); b.whileTrue(m_swerve.setRotationCommand(180)); From 8e605544a74b50bf8415e8d6ce845b951515f6f2 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Thu, 14 Mar 2024 18:02:22 -0400 Subject: [PATCH 07/13] Add pre-spin-up --- src/main/java/frc/robot/Constants.java | 9 ++++ src/main/java/frc/robot/RobotContainer.java | 20 +++++++- .../frc/robot/commands/PassiveLaunchSpin.java | 46 +++++++++++++++++++ .../java/frc/robot/subsystems/Launcher.java | 26 +++++++++++ 4 files changed, 99 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/commands/PassiveLaunchSpin.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 23d42b1d..e209c312 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -78,11 +78,20 @@ public static class Launch { public static final double speedFarSpeaker = 2800; public static final double angleConversionFactor = 1000; public static final double launcherConversionFactor = 1; + public static final double safeLaunchVelo = 6000; + public static final double subwooferLaunchVelo = 3500; + public static final double ampLaunchVelo = 1500; public enum LaunchPosition { CLOSE, FAR } + + public enum LaunchPreset { + SUBWOOFER, + SAFE, + AMP + } } public static class Climb { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 99f3117c..2dc25bdf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,13 +15,16 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.Launch.LaunchPreset; import frc.robot.Constants.OperatorConstants; import frc.robot.commands.AbsoluteDrive; import frc.robot.commands.AbsoluteDriveAdv; import frc.robot.commands.AlignLaunchAuto; import frc.robot.commands.LaunchWithVelo; import frc.robot.commands.LaunchWithVeloAuton; +import frc.robot.commands.PassiveLaunchSpin; import frc.robot.commands.PrimeIndex; import frc.robot.commands.SwitchLaunchAngle; import frc.robot.commands.ToggleIntake; @@ -109,11 +112,15 @@ public RobotContainer() { () -> driverLeft.getAsBoolean(), () -> driverRight.getAsBoolean()); - m_swerve.setDefaultCommand(!RobotBase.isSimulation() ? closedAbsoluteDrive : advancedDrive); + m_swerve.setDefaultCommand( + !RobotBase.isSimulation() ? closedAbsoluteDrive : closedAbsoluteDrive); + m_launch.setDefaultCommand(new PassiveLaunchSpin(m_launch, m_index)); // -m_index.setDefaultCommand(m_index.manualIntake(coDriver::getRightY)); - m_climb.setDefaultCommand(m_climb.setWinchCommand(() -> MathUtil.applyDeadband(coDriver.getLeftY(), Constants.Climb.deadzone))); + m_climb.setDefaultCommand( + m_climb.setWinchCommand( + () -> MathUtil.applyDeadband(coDriver.getLeftY(), Constants.Climb.deadzone))); // add auto options m_chooser.setDefaultOption("Test Drive", m_swerve.getAutonomousCommand("Test Drive")); @@ -173,6 +180,15 @@ private void configureBindings() { JoystickButton b = new JoystickButton(driver, XboxController.Button.kB.value); b.whileTrue(m_swerve.setRotationCommand(180)); + + POVButton up = new POVButton(driver, 0); + up.onTrue(m_launch.setLaunchPresetCommand(LaunchPreset.SAFE)); + + POVButton down = new POVButton(driver, 180); + down.onTrue(m_launch.setLaunchPresetCommand(LaunchPreset.SUBWOOFER)); + + POVButton left = new POVButton(driver, 270); + left.onTrue(m_launch.setLaunchPresetCommand(LaunchPreset.AMP)); } /** diff --git a/src/main/java/frc/robot/commands/PassiveLaunchSpin.java b/src/main/java/frc/robot/commands/PassiveLaunchSpin.java new file mode 100644 index 00000000..0fe38f83 --- /dev/null +++ b/src/main/java/frc/robot/commands/PassiveLaunchSpin.java @@ -0,0 +1,46 @@ +// 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.Index; +import frc.robot.subsystems.Launcher; + +public class PassiveLaunchSpin extends Command { + private Launcher m_launcher; + private Index m_index; + + /** Creates a new PassiveLaunchSpin. */ + public PassiveLaunchSpin(Launcher launcher, Index index) { + m_launcher = launcher; + m_index = index; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(m_launcher); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (m_index.getUpperSensorHit()) { + m_launcher.setPresetVelo(); + } else { + m_launcher.setLaunchVelocity(0); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Launcher.java b/src/main/java/frc/robot/subsystems/Launcher.java index e8948a2e..020185d3 100644 --- a/src/main/java/frc/robot/subsystems/Launcher.java +++ b/src/main/java/frc/robot/subsystems/Launcher.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Constants.Launch.LaunchPosition; +import frc.robot.Constants.Launch.LaunchPreset; import frc.robot.Robot; public class Launcher extends SubsystemBase { @@ -40,6 +41,8 @@ public class Launcher extends SubsystemBase { private LaunchPosition goalPosition, curPosition; + private double goalVelo; + public Launcher() { angle.setInverted(Constants.Launch.angleMotorInverted); @@ -76,6 +79,8 @@ public Launcher() { angleController.setSmartMotionMinOutputVelocity(0, 0); angleController.setSmartMotionMaxAccel(1500, 0); angleController.setSmartMotionAllowedClosedLoopError(0, 0); + + goalVelo = Constants.Launch.farLaunchPosition; } private void showPIDs() { @@ -179,6 +184,23 @@ public void togglePIDTuning() { tuningPIDS = !tuningPIDS; } + public void setLaunchPreset(LaunchPreset preset) { + if (preset == LaunchPreset.SUBWOOFER) { + goalPosition = LaunchPosition.CLOSE; + goalVelo = Constants.Launch.subwooferLaunchVelo; + } else if (preset == LaunchPreset.SAFE) { + goalPosition = LaunchPosition.FAR; + goalVelo = Constants.Launch.safeLaunchVelo; + } else if (preset == LaunchPreset.AMP) { + goalPosition = LaunchPosition.CLOSE; + goalVelo = Constants.Launch.ampLaunchVelo; + } + } + + public void setPresetVelo() { + setLaunchVelocity(goalVelo); + } + @FunctionalInterface private interface MyMethod { void apply(double p, double i, double d, double ff); @@ -216,4 +238,8 @@ public void lockLauncher() { public Command switchAngleCommand() { return this.runOnce(() -> switchAngle()); } + + public Command setLaunchPresetCommand(LaunchPreset preset) { + return this.runOnce(() -> setLaunchPreset(preset)); + } } From dbe182458f72333bf62d7ea8e4f97b727ec6e9d2 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Thu, 14 Mar 2024 20:43:09 -0400 Subject: [PATCH 08/13] Put current velos on dashboard --- src/main/java/frc/robot/subsystems/Launcher.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Launcher.java b/src/main/java/frc/robot/subsystems/Launcher.java index 020185d3..6663e9d0 100644 --- a/src/main/java/frc/robot/subsystems/Launcher.java +++ b/src/main/java/frc/robot/subsystems/Launcher.java @@ -223,7 +223,8 @@ public void periodic() { SmartDashboard.putNumber("Angle Desired", Constants.Launch.closeLaunchPosition); } SmartDashboard.putNumber("Angle Speed", angle.get()); - SmartDashboard.putNumber("Launch Curr Velo", getCurrentVelocity()); + SmartDashboard.putNumber("Launch Curr Velo Lower", upperLauncherEncoder.getVelocity()); + SmartDashboard.putNumber("Launch Curr Velo Upper", lowerLauncherEncoder.getVelocity()); SmartDashboard.putNumber("Angle Position", angleEncoder.getPosition()); } From e275a34282d4136b90aae80977332e0a0d4f6296 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Thu, 14 Mar 2024 20:43:23 -0400 Subject: [PATCH 09/13] Change right intake to kraken --- src/main/java/frc/robot/subsystems/Intake.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 0e5e96ae..2fffed15 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -4,6 +4,7 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; import edu.wpi.first.math.filter.SlewRateLimiter; @@ -19,8 +20,8 @@ public class Intake extends SubsystemBase { private CANSparkMax motorCenter = new CANSparkMax(Constants.Intake.centerCANID, CANSparkLowLevel.MotorType.kBrushless); - private CANSparkMax motorRight = - new CANSparkMax(Constants.Intake.rightCANID, CANSparkLowLevel.MotorType.kBrushless); + private TalonFX motorRight = + new TalonFX(Constants.Intake.rightCANID); private SlewRateLimiter limiter = new SlewRateLimiter(0.5); From 2bbe4baa888a326c7559d21bffd314455a9523ea Mon Sep 17 00:00:00 2001 From: robo7660 Date: Thu, 14 Mar 2024 20:43:47 -0400 Subject: [PATCH 10/13] Retune angle again --- src/main/java/frc/robot/Constants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e209c312..8e8a0e9c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -67,10 +67,10 @@ public static class Launch { public static final int angleID = 46; public static final double allowedVeloPercent = 10; public static final double allowedDifferencePercent = 5; - public static final double closeLaunchPosition = 330; - public static final double farLaunchPosition = 262; - public static final double angleMin = 261; - public static final double angleMax = 333; + public static final double closeLaunchPosition = 828; + public static final double farLaunchPosition = 752; + public static final double angleMin = 750; + public static final double angleMax = 828; public static final boolean angleMotorInverted = true; public static final boolean angleEncoderInverted = false; public static final boolean lowerMotorInverted = true; From 06609ad615b8969bccd75ba7e876c80373167562 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Thu, 14 Mar 2024 20:44:00 -0400 Subject: [PATCH 11/13] More Consistent autos --- .../pathplanner/autos/Short Shot Amp.auto | 31 +++++++++++++++++++ .../pathplanner/autos/Short Shot Center.auto | 31 +++++++++++++++++++ .../pathplanner/autos/Short Shot Safe.auto | 31 +++++++++++++++++++ .../pathplanner/paths/Simple Shoot Amp 3.path | 4 +-- .../paths/Simple Shoot Center 2.path | 16 +++++----- .../paths/Simple Shoot Safe 2.path | 16 +++++----- src/main/java/frc/robot/RobotContainer.java | 16 +++++----- 7 files changed, 118 insertions(+), 27 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Short Shot Amp.auto create mode 100644 src/main/deploy/pathplanner/autos/Short Shot Center.auto create mode 100644 src/main/deploy/pathplanner/autos/Short Shot Safe.auto diff --git a/src/main/deploy/pathplanner/autos/Short Shot Amp.auto b/src/main/deploy/pathplanner/autos/Short Shot Amp.auto new file mode 100644 index 00000000..cb747781 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Short Shot Amp.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6946944060158917, + "y": 6.727333169630711 + }, + "rotation": -120.0013184955057 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "subwoofer-launch" + } + }, + { + "type": "path", + "data": { + "pathName": "Simple Shoot Amp 3" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Short Shot Center.auto b/src/main/deploy/pathplanner/autos/Short Shot Center.auto new file mode 100644 index 00000000..7f3acf91 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Short Shot Center.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.367427722973922, + "y": 5.5866114582541275 + }, + "rotation": 179.5103044068707 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "subwoofer-launch" + } + }, + { + "type": "path", + "data": { + "pathName": "Simple Shoot Center 2" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Short Shot Safe.auto b/src/main/deploy/pathplanner/autos/Short Shot Safe.auto new file mode 100644 index 00000000..6f721cf4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Short Shot Safe.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.77430354290908, + "y": 4.303448834817898 + }, + "rotation": 120.48894049983096 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "subwoofer-launch" + } + }, + { + "type": "path", + "data": { + "pathName": "Simple Shoot Safe 2" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Amp 3.path b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 3.path index 670c8d98..314fd20a 100644 --- a/src/main/deploy/pathplanner/paths/Simple Shoot Amp 3.path +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 3.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 7.480526124947738, + "x": 8.475001463067061, "y": 6.92 }, "prevControl": { - "x": 6.480526124947738, + "x": 7.475001463067061, "y": 6.92 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Center 2.path b/src/main/deploy/pathplanner/paths/Simple Shoot Center 2.path index 27f7c353..c3b73dfd 100644 --- a/src/main/deploy/pathplanner/paths/Simple Shoot Center 2.path +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Center 2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.26, - "y": 5.55 + "x": 1.386927239412675, + "y": 5.615860732909115 }, "prevControl": null, "nextControl": { - "x": 3.2588808170490466, - "y": 6.629835587462149 + "x": 1.385808056461722, + "y": 6.6956963203712645 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.773018871453421, - "y": 7.07832446543753 + "x": 8.3775038808985, + "y": 7.088074223654385 }, "prevControl": { - "x": 6.773018871453421, - "y": 7.07832446543753 + "x": 7.3775038808985, + "y": 7.088074223654385 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path index ffb2c456..91f5a1cb 100644 --- a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.58, - "y": 3.02 + "x": 1.8256663591711995, + "y": 2.47643858708145 }, "prevControl": null, "nextControl": { - "x": 2.63367409925703, - "y": 1.7073465485962325 + "x": 1.8451658756049114, + "y": 2.47643858708145 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.925428779632488, - "y": 0.6665668032190769 + "x": 8.29950581516365, + "y": 1.1894705024564445 }, "prevControl": { - "x": 6.200391204201211, - "y": 0.7016492665463968 + "x": 7.626772498200579, + "y": 0.7312318662642086 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2dc25bdf..01dff83d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -68,6 +68,7 @@ public RobotContainer() { NamedCommands.registerCommand("index", new PrimeIndex(m_index, m_transfer)); NamedCommands.registerCommand( "align-launch", new AlignLaunchAuto(m_swerve, m_launch, m_index, 5300, 1)); + NamedCommands.registerCommand("subwoofer-launch", new AlignLaunchAuto(m_swerve, m_launch, m_index, 3200, 1)); NamedCommands.registerCommand("reverse intake", m_intake.reverseIntakeCommand()); CameraServer.startAutomaticCapture(0); @@ -126,19 +127,16 @@ public RobotContainer() { m_chooser.setDefaultOption("Test Drive", m_swerve.getAutonomousCommand("Test Drive")); m_chooser.addOption("Mid 2 Piece", m_swerve.getAutonomousCommand("Mid 2 Piece")); - m_chooser.addOption("Turn Auto", m_swerve.getAutonomousCommand("Turn Auto")); - m_chooser.addOption("Drive and Turn", m_swerve.getAutonomousCommand("drive and turn")); m_chooser.addOption("1 Centerline", m_swerve.getAutonomousCommand("1 Centerline")); m_chooser.addOption("5 Centerline", m_swerve.getAutonomousCommand("5 Centerline")); m_chooser.addOption("Close 2", m_swerve.getAutonomousCommand("Close 2")); + m_chooser.addOption("Just Shoot", new AlignLaunchAuto(m_swerve, m_launch, m_index, 3200, 1)); m_chooser.addOption("Just Chill", m_swerve.noAuto()); m_chooser.addOption( - "Simple Shoot Center", m_swerve.getAutonomousCommand("Simple Shoot Center")); + "Short Shot Center", m_swerve.getAutonomousCommand("Short Shot Center")); m_chooser.addOption( - "Simple Shoot Safe Blue", m_swerve.getAutonomousCommand("Simple Shoot Safe Blue")); - m_chooser.addOption("Simple Shoot Amp", m_swerve.getAutonomousCommand("Simple Shoot Amp")); - m_chooser.addOption( - "Simple Shoot Safe Red", m_swerve.getAutonomousCommand("Simple Shoot Safe Red")); + "Short Shot Safe", m_swerve.getAutonomousCommand("Short Shot Safe")); + m_chooser.addOption("Short Shot Amp", m_swerve.getAutonomousCommand("Short Shot Amp")); SmartDashboard.putData(m_chooser); } @@ -173,13 +171,13 @@ private void configureBindings() { x.whileTrue(m_swerve.updatePositionCommand()); JoystickButton a = new JoystickButton(driver, XboxController.Button.kA.value); - a.whileTrue(new LaunchWithVelo(m_launch, m_index, 2050, false)); + a.whileTrue(new LaunchWithVelo(m_launch, m_index, 3500, false)); JoystickButton y = new JoystickButton(driver, XboxController.Button.kY.value); y.whileTrue(new SwitchLaunchAngle(m_launch)); JoystickButton b = new JoystickButton(driver, XboxController.Button.kB.value); - b.whileTrue(m_swerve.setRotationCommand(180)); + b.whileTrue(new LaunchWithVelo(m_launch, m_index, 1500, true)); POVButton up = new POVButton(driver, 0); up.onTrue(m_launch.setLaunchPresetCommand(LaunchPreset.SAFE)); From 98090d2acecabe79fb87ef1d38d08459a23a0f0e Mon Sep 17 00:00:00 2001 From: robo7660 Date: Fri, 15 Mar 2024 12:41:41 -0400 Subject: [PATCH 12/13] Better autos maybe --- .../deploy/pathplanner/paths/Simple Shoot Safe 2.path | 8 ++++---- src/main/java/frc/robot/RobotContainer.java | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path index 91f5a1cb..19dcfd62 100644 --- a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.8256663591711995, - "y": 2.47643858708145 + "x": 2.1571581385443066, + "y": 1.4234646996609919 }, "prevControl": null, "nextControl": { - "x": 1.8451658756049114, - "y": 2.47643858708145 + "x": 2.176657654978018, + "y": 1.4234646996609919 }, "isLocked": false, "linkedName": null diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 01dff83d..562eff7c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -68,7 +68,7 @@ public RobotContainer() { NamedCommands.registerCommand("index", new PrimeIndex(m_index, m_transfer)); NamedCommands.registerCommand( "align-launch", new AlignLaunchAuto(m_swerve, m_launch, m_index, 5300, 1)); - NamedCommands.registerCommand("subwoofer-launch", new AlignLaunchAuto(m_swerve, m_launch, m_index, 3200, 1)); + NamedCommands.registerCommand("subwoofer-launch", new AlignLaunchAuto(m_swerve, m_launch, m_index, 3500, 2)); NamedCommands.registerCommand("reverse intake", m_intake.reverseIntakeCommand()); CameraServer.startAutomaticCapture(0); From faedb6492200965cfca6238343b35c444fa84d2d Mon Sep 17 00:00:00 2001 From: robo7660 Date: Fri, 15 Mar 2024 12:50:13 -0400 Subject: [PATCH 13/13] Ran Spotless --- src/main/java/frc/robot/RobotContainer.java | 9 ++++----- src/main/java/frc/robot/subsystems/Intake.java | 3 +-- src/main/java/frc/robot/subsystems/Launcher.java | 2 +- 3 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 562eff7c..1899226a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -68,7 +68,8 @@ public RobotContainer() { NamedCommands.registerCommand("index", new PrimeIndex(m_index, m_transfer)); NamedCommands.registerCommand( "align-launch", new AlignLaunchAuto(m_swerve, m_launch, m_index, 5300, 1)); - NamedCommands.registerCommand("subwoofer-launch", new AlignLaunchAuto(m_swerve, m_launch, m_index, 3500, 2)); + NamedCommands.registerCommand( + "subwoofer-launch", new AlignLaunchAuto(m_swerve, m_launch, m_index, 3500, 2)); NamedCommands.registerCommand("reverse intake", m_intake.reverseIntakeCommand()); CameraServer.startAutomaticCapture(0); @@ -132,10 +133,8 @@ public RobotContainer() { m_chooser.addOption("Close 2", m_swerve.getAutonomousCommand("Close 2")); m_chooser.addOption("Just Shoot", new AlignLaunchAuto(m_swerve, m_launch, m_index, 3200, 1)); m_chooser.addOption("Just Chill", m_swerve.noAuto()); - m_chooser.addOption( - "Short Shot Center", m_swerve.getAutonomousCommand("Short Shot Center")); - m_chooser.addOption( - "Short Shot Safe", m_swerve.getAutonomousCommand("Short Shot Safe")); + m_chooser.addOption("Short Shot Center", m_swerve.getAutonomousCommand("Short Shot Center")); + m_chooser.addOption("Short Shot Safe", m_swerve.getAutonomousCommand("Short Shot Safe")); m_chooser.addOption("Short Shot Amp", m_swerve.getAutonomousCommand("Short Shot Amp")); SmartDashboard.putData(m_chooser); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 2fffed15..8695c98f 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -20,8 +20,7 @@ public class Intake extends SubsystemBase { private CANSparkMax motorCenter = new CANSparkMax(Constants.Intake.centerCANID, CANSparkLowLevel.MotorType.kBrushless); - private TalonFX motorRight = - new TalonFX(Constants.Intake.rightCANID); + private TalonFX motorRight = new TalonFX(Constants.Intake.rightCANID); private SlewRateLimiter limiter = new SlewRateLimiter(0.5); diff --git a/src/main/java/frc/robot/subsystems/Launcher.java b/src/main/java/frc/robot/subsystems/Launcher.java index 6663e9d0..0303f9bc 100644 --- a/src/main/java/frc/robot/subsystems/Launcher.java +++ b/src/main/java/frc/robot/subsystems/Launcher.java @@ -224,7 +224,7 @@ public void periodic() { } SmartDashboard.putNumber("Angle Speed", angle.get()); SmartDashboard.putNumber("Launch Curr Velo Lower", upperLauncherEncoder.getVelocity()); - SmartDashboard.putNumber("Launch Curr Velo Upper", lowerLauncherEncoder.getVelocity()); + SmartDashboard.putNumber("Launch Curr Velo Upper", lowerLauncherEncoder.getVelocity()); SmartDashboard.putNumber("Angle Position", angleEncoder.getPosition()); }