From d08ef70cf399091c6042240bf1d8eed4340b5746 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Fri, 1 Mar 2024 07:49:01 -0500 Subject: [PATCH 01/30] Move lower index to transfer subsyste, --- src/main/java/frc/robot/RobotContainer.java | 13 +++--- .../java/frc/robot/commands/PrimeIndex.java | 17 +++---- .../java/frc/robot/commands/ToggleIntake.java | 6 ++- src/main/java/frc/robot/subsystems/Index.java | 15 +------ .../java/frc/robot/subsystems/Transfer.java | 45 +++++++++++++++++++ 5 files changed, 66 insertions(+), 30 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Transfer.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f5933b4d..4673b872 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,8 @@ import frc.robot.subsystems.Intake; import frc.robot.subsystems.Launcher; import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.subsystems.Transfer; + import java.io.File; /** @@ -45,6 +47,7 @@ public class RobotContainer { private final Index m_index = new Index(); private final Launcher m_launch = new Launcher(); private final Climb m_climb = new Climb(); + private final Transfer m_transfer = new Transfer(); // Replace with CommandPS4Controller or CommandJoystick if needed private final XboxController driver = new XboxController(0); @@ -56,8 +59,8 @@ public RobotContainer() { // Create commands for PathPlanner NamedCommands.registerCommand( "launch", new LaunchWithVeloAuton(m_launch, m_index, Constants.Launch.speedFarSpeaker)); - NamedCommands.registerCommand("intake", new ToggleIntake(m_intake)); - NamedCommands.registerCommand("index", new PrimeIndex(m_index)); + NamedCommands.registerCommand("intake", new ToggleIntake(m_intake, m_transfer)); + NamedCommands.registerCommand("index", new PrimeIndex(m_index, m_transfer)); NamedCommands.registerCommand( "align-launch", new AlignLaunchAuto(m_swerve, m_launch, m_index, 3000, 1)); NamedCommands.registerCommand("reverse intake", m_intake.reverseIntakeCommand()); @@ -113,16 +116,16 @@ public RobotContainer() { private void configureBindings() { JoystickButton leftBumper = new JoystickButton(driver, XboxController.Button.kLeftBumper.value); - leftBumper.onTrue(new ToggleIntake(m_intake)); + leftBumper.onTrue(new ToggleIntake(m_intake, m_transfer)); JoystickButton coRB = new JoystickButton(coDriver, XboxController.Button.kRightBumper.value); - coRB.onTrue(new PrimeIndex(m_index)); + coRB.onTrue(new PrimeIndex(m_index, m_transfer)); Trigger lt = new Trigger(() -> driver.getLeftTriggerAxis() >= 0.05); lt.whileTrue(m_swerve.alignCommand()); Trigger rt = new Trigger(() -> driver.getRightTriggerAxis() >= 0.05); - rt.whileTrue(new LaunchWithVelo(m_launch, m_index, 5200, false)); + rt.whileTrue(new LaunchWithVelo(m_launch, m_index, 2500, false)); JoystickButton x = new JoystickButton(driver, XboxController.Button.kX.value); x.onTrue(m_swerve.updatePositionCommand()); diff --git a/src/main/java/frc/robot/commands/PrimeIndex.java b/src/main/java/frc/robot/commands/PrimeIndex.java index f42f71a9..7eb173e3 100644 --- a/src/main/java/frc/robot/commands/PrimeIndex.java +++ b/src/main/java/frc/robot/commands/PrimeIndex.java @@ -6,9 +6,11 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Index; +import frc.robot.subsystems.Transfer; public class PrimeIndex extends Command { private final Index index; + private final Transfer transfer; private boolean inUpper; private boolean wasRunning; @@ -17,8 +19,9 @@ public class PrimeIndex extends Command { * * @param subsystem The subsystem used by this command. */ - public PrimeIndex(Index index) { + public PrimeIndex(Index index, Transfer transfer) { this.index = index; + this.transfer = transfer; inUpper = false; wasRunning = index.isRunning(); // Use addRequirements() here to declare subsystem dependencies. @@ -28,12 +31,9 @@ public PrimeIndex(Index index) { // Called when the command is initially scheduled. @Override public void initialize() { - if (!wasRunning) { System.out.println("Index is initializing"); index.start(); - } else { - index.stop(); - } + transfer.start(); } // Called every time the scheduler runs while the command is scheduled. @@ -41,7 +41,7 @@ public void initialize() { public void execute() { if (!inUpper && index.isInUpper()) { inUpper = true; - index.setLower(0); + transfer.stop(); } } @@ -54,10 +54,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - if (wasRunning) { - return true; - } else { return index.isPrimed(); - } } } + diff --git a/src/main/java/frc/robot/commands/ToggleIntake.java b/src/main/java/frc/robot/commands/ToggleIntake.java index 73afa229..f3a8d0a0 100644 --- a/src/main/java/frc/robot/commands/ToggleIntake.java +++ b/src/main/java/frc/robot/commands/ToggleIntake.java @@ -6,13 +6,16 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Transfer; public class ToggleIntake extends Command { private Intake intake; + private Transfer transfer; /** Creates a new ToggleIntake. */ - public ToggleIntake(Intake intake) { + public ToggleIntake(Intake intake, Transfer transfer) { this.intake = intake; + this.transfer = transfer; addRequirements(intake); } @@ -21,6 +24,7 @@ public ToggleIntake(Intake intake) { public void initialize() { System.out.println("Intake is initializing"); intake.toggle(); + transfer.toggle(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/Index.java b/src/main/java/frc/robot/subsystems/Index.java index 1e47632a..f925b575 100644 --- a/src/main/java/frc/robot/subsystems/Index.java +++ b/src/main/java/frc/robot/subsystems/Index.java @@ -12,8 +12,6 @@ import frc.robot.Constants; public class Index extends SubsystemBase { - private CANSparkMax motorLower = - new CANSparkMax(Constants.Index.lowerCANID, CANSparkLowLevel.MotorType.kBrushless); private CANSparkMax motorWhooper = new CANSparkMax(Constants.Index.whooperCANID, CANSparkLowLevel.MotorType.kBrushless); private CANSparkMax motorUpper = @@ -25,20 +23,17 @@ public class Index extends SubsystemBase { double speed = 0.95; public Index() { - motorLower.setInverted(Constants.Index.lowerInverted); motorWhooper.setInverted(Constants.Index.whooperInverted); motorUpper.setInverted(Constants.Index.upperInverted); } private void set(double power) { - motorLower.set(power); motorWhooper.set(power); motorUpper.set(power - 0.5); currentSpeed = power; } public void run() { - runLower(); runWhooper(); runUpper(); } @@ -51,10 +46,6 @@ public void setUpper(double speed) { motorUpper.set(speed); } - public void setLower(double speed) { - motorLower.set(speed); - } - public void setWhooper(double speed) { motorWhooper.set(speed); } @@ -67,10 +58,6 @@ public void runWhooper() { setWhooper(Constants.Index.whooperSpeed); } - public void runLower() { - setLower(Constants.Index.lowerSpeed); - } - public boolean isRunning() { return !(motorUpper.get() == 0); } @@ -80,7 +67,6 @@ public void start() { } public void stop() { - motorLower.set(0); motorWhooper.set(0); motorUpper.set(0); } @@ -121,6 +107,7 @@ 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(); } diff --git a/src/main/java/frc/robot/subsystems/Transfer.java b/src/main/java/frc/robot/subsystems/Transfer.java new file mode 100644 index 00000000..ff6196e0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Transfer.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.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class Transfer extends SubsystemBase { + /** Creates a new Transfer. */ + CANSparkMax motor = new CANSparkMax(Constants.Index.lowerCANID, MotorType.kBrushless); + private double currentSpeed; + + public Transfer() { + motor.setInverted(Constants.Index.lowerInverted); + currentSpeed = 0; + } + + public void start() { + motor.set(Constants.Index.lowerSpeed); + currentSpeed = Constants.Index.lowerSpeed; + } + + public void stop() { + motor.set(0); + currentSpeed = 0; + } + + public void toggle() { + if (currentSpeed == 0.0) { + start(); + } else { + stop(); + } + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From 36bf91734272201bb242f2852dabc132b7afae95 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Fri, 1 Mar 2024 07:52:30 -0500 Subject: [PATCH 02/30] Fixed Constants and whitespace --- src/main/java/frc/robot/Constants.java | 9 ++++++--- src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/commands/PrimeIndex.java | 9 ++++----- src/main/java/frc/robot/subsystems/Transfer.java | 12 ++++++------ 4 files changed, 16 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index efdc93a1..c60f38f2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,15 +33,12 @@ public final class Constants { public static final PIDConstants autoRotationPID = new PIDConstants(0.4, 0, 0.01); public static class Index { - public static final int lowerCANID = 40; public static final int whooperCANID = 41; public static final int upperCANID = 42; - public static final boolean lowerInverted = true; public static final boolean upperInverted = true; public static final boolean whooperInverted = false; public static final int upperBeam = 1; public static final int lowerBeam = 0; - public static final double lowerSpeed = 0.15; public static final double whooperSpeed = 0.9; public static final double upperSpeed = 0.3; } @@ -98,6 +95,12 @@ public static class Climb { public static final double motorSpeedFactor = -0.5; } + public static class Transfer { + public static final boolean motorInverted = true; + public static final double motorSpeed = 0.15; + public static final int motorID = 40; + } + public static final String limelightName = "limelight"; public static class OperatorConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4673b872..11970555 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,7 +29,6 @@ import frc.robot.subsystems.Launcher; import frc.robot.subsystems.SwerveSubsystem; import frc.robot.subsystems.Transfer; - import java.io.File; /** diff --git a/src/main/java/frc/robot/commands/PrimeIndex.java b/src/main/java/frc/robot/commands/PrimeIndex.java index 7eb173e3..b1b561f1 100644 --- a/src/main/java/frc/robot/commands/PrimeIndex.java +++ b/src/main/java/frc/robot/commands/PrimeIndex.java @@ -31,9 +31,9 @@ public PrimeIndex(Index index, Transfer transfer) { // Called when the command is initially scheduled. @Override public void initialize() { - System.out.println("Index is initializing"); - index.start(); - transfer.start(); + System.out.println("Index is initializing"); + index.start(); + transfer.start(); } // Called every time the scheduler runs while the command is scheduled. @@ -54,7 +54,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return index.isPrimed(); + return index.isPrimed(); } } - diff --git a/src/main/java/frc/robot/subsystems/Transfer.java b/src/main/java/frc/robot/subsystems/Transfer.java index ff6196e0..3ab72904 100644 --- a/src/main/java/frc/robot/subsystems/Transfer.java +++ b/src/main/java/frc/robot/subsystems/Transfer.java @@ -4,25 +4,25 @@ package frc.robot.subsystems; -import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkLowLevel.MotorType; - +import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; public class Transfer extends SubsystemBase { /** Creates a new Transfer. */ - CANSparkMax motor = new CANSparkMax(Constants.Index.lowerCANID, MotorType.kBrushless); + CANSparkMax motor = new CANSparkMax(Constants.Transfer.motorID, MotorType.kBrushless); + private double currentSpeed; public Transfer() { - motor.setInverted(Constants.Index.lowerInverted); + motor.setInverted(Constants.Transfer.motorInverted); currentSpeed = 0; } public void start() { - motor.set(Constants.Index.lowerSpeed); - currentSpeed = Constants.Index.lowerSpeed; + motor.set(Constants.Transfer.motorSpeed); + currentSpeed = Constants.Transfer.motorSpeed; } public void stop() { From dbcc50ab29e5b7d4ac55ed898451063362430b12 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Sat, 2 Mar 2024 09:36:01 -0500 Subject: [PATCH 03/30] Auton Reworks --- .../pathplanner/autos/Simple Shoot Amp.auto | 43 +++++++++++++++ .../autos/Simple Shoot Center.auto | 37 +++++++++++++ .../pathplanner/autos/Simple Shoot Safe.auto | 37 +++++++++++++ .../pathplanner/paths/Mid 2 Piece 1.path | 10 +++- .../pathplanner/paths/Simple Shoot Amp 1.path | 52 +++++++++++++++++++ .../pathplanner/paths/Simple Shoot Amp 2.path | 52 +++++++++++++++++++ .../pathplanner/paths/Simple Shoot Amp 3.path | 52 +++++++++++++++++++ .../paths/Simple Shoot Center 1.path | 52 +++++++++++++++++++ .../paths/Simple Shoot Center 2.path | 52 +++++++++++++++++++ .../paths/Simple Shoot Safe 1.path | 52 +++++++++++++++++++ .../paths/Simple Shoot Safe 2.path | 52 +++++++++++++++++++ 11 files changed, 489 insertions(+), 2 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Simple Shoot Amp.auto create mode 100644 src/main/deploy/pathplanner/autos/Simple Shoot Center.auto create mode 100644 src/main/deploy/pathplanner/autos/Simple Shoot Safe.auto create mode 100644 src/main/deploy/pathplanner/paths/Simple Shoot Amp 1.path create mode 100644 src/main/deploy/pathplanner/paths/Simple Shoot Amp 2.path create mode 100644 src/main/deploy/pathplanner/paths/Simple Shoot Amp 3.path create mode 100644 src/main/deploy/pathplanner/paths/Simple Shoot Center 1.path create mode 100644 src/main/deploy/pathplanner/paths/Simple Shoot Center 2.path create mode 100644 src/main/deploy/pathplanner/paths/Simple Shoot Safe 1.path create mode 100644 src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path diff --git a/src/main/deploy/pathplanner/autos/Simple Shoot Amp.auto b/src/main/deploy/pathplanner/autos/Simple Shoot Amp.auto new file mode 100644 index 00000000..9df65889 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Simple Shoot Amp.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6946944060158917, + "y": 6.727333169630711 + }, + "rotation": -120.0013184955057 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Simple Shoot Amp 1" + } + }, + { + "type": "path", + "data": { + "pathName": "Simple Shoot Amp 2" + } + }, + { + "type": "named", + "data": { + "name": "align-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/Simple Shoot Center.auto b/src/main/deploy/pathplanner/autos/Simple Shoot Center.auto new file mode 100644 index 00000000..34fff746 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Simple Shoot Center.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.367427722973922, + "y": 5.5866114582541275 + }, + "rotation": 179.5103044068707 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Simple Shoot Center 1" + } + }, + { + "type": "named", + "data": { + "name": "align-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/Simple Shoot Safe.auto b/src/main/deploy/pathplanner/autos/Simple Shoot Safe.auto new file mode 100644 index 00000000..5ad177e5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Simple Shoot Safe.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7336934388833163, + "y": 4.406890714018958 + }, + "rotation": 120.48894049983096 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Simple Shoot Safe 1" + } + }, + { + "type": "named", + "data": { + "name": "align-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/Mid 2 Piece 1.path b/src/main/deploy/pathplanner/paths/Mid 2 Piece 1.path index 6d09c6e6..e60a2102 100644 --- a/src/main/deploy/pathplanner/paths/Mid 2 Piece 1.path +++ b/src/main/deploy/pathplanner/paths/Mid 2 Piece 1.path @@ -28,7 +28,13 @@ "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 132.00142152168155, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -39,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 139.02826366648515, + "rotation": 135.73452103426732, "rotateFast": true }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Amp 1.path b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 1.path new file mode 100644 index 00000000..c80da93f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.6946944060158917, + "y": 6.727333169630711 + }, + "prevControl": null, + "nextControl": { + "x": 0.6946944060158917, + "y": 6.717583411413854 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0554354600395677, + "y": 7.283069387991508 + }, + "prevControl": { + "x": 1.0651852182564236, + "y": 7.283069387991508 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -123.3484442689218, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -119.87932558561347, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Amp 2.path b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 2.path new file mode 100644 index 00000000..20a6dd98 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.06, + "y": 7.28 + }, + "prevControl": null, + "nextControl": { + "x": 1.0261861853889993, + "y": 7.2538201133409395 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9273890376759386, + "y": 6.932078092184688 + }, + "prevControl": { + "x": 1.9273890376759386, + "y": 6.932078092184688 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -134.11859600341796, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -123.35467894844413, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ 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 new file mode 100644 index 00000000..0945df8e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 3.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.82, + "y": 6.92 + }, + "prevControl": null, + "nextControl": { + "x": 3.82, + "y": 6.92 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.480526124947738, + "y": 6.92 + }, + "prevControl": { + "x": 6.480526124947738, + "y": 6.92 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -151.35337731670384, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Center 1.path b/src/main/deploy/pathplanner/paths/Simple Shoot Center 1.path new file mode 100644 index 00000000..b3802ec7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Center 1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3381784483283943, + "y": 5.547612425391122 + }, + "prevControl": null, + "nextControl": { + "x": 2.338178448328395, + "y": 5.547612425391122 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.2588808170490466, + "y": 5.547612425391122 + }, + "prevControl": { + "x": 2.2588808170490466, + "y": 5.547612425391122 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 178.8982938847937, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 178.70783311375007, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Center 2.path b/src/main/deploy/pathplanner/paths/Simple Shoot Center 2.path new file mode 100644 index 00000000..c820da0e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Center 2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.26, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 3.2588808170490466, + "y": 6.629835587462149 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.773018871453421, + "y": 7.07832446543753 + }, + "prevControl": { + "x": 6.773018871453421, + "y": 7.07832446543753 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 177.79740183823418, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -179.67887802899924, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1.path b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1.path new file mode 100644 index 00000000..33e2214a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7336934388833163, + "y": 4.406890714018958 + }, + "prevControl": null, + "nextControl": { + "x": 0.7336934388833163, + "y": 4.37764143936839 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.7184190187857824, + "y": 2.856679157538838 + }, + "prevControl": { + "x": 1.689169744135214, + "y": 2.895678190406262 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 99.1301764822787, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 122.23888483747, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path new file mode 100644 index 00000000..b28165ef --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.72, + "y": 2.86 + }, + "prevControl": null, + "nextControl": { + "x": 2.7199999999999998, + "y": 2.8599999999999994 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.266031444176903, + "y": 1.1309719531553086 + }, + "prevControl": { + "x": 6.266031444176903, + "y": 1.1309719531553086 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -58.37925702479779, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file From 5824ecfdb49943c4392580e819d8bdd377176028 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Sat, 2 Mar 2024 09:36:19 -0500 Subject: [PATCH 04/30] Add reverse for intake through launcher --- src/main/java/frc/robot/commands/LaunchWithVelo.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/LaunchWithVelo.java b/src/main/java/frc/robot/commands/LaunchWithVelo.java index ac90270e..721fe2d5 100644 --- a/src/main/java/frc/robot/commands/LaunchWithVelo.java +++ b/src/main/java/frc/robot/commands/LaunchWithVelo.java @@ -42,7 +42,11 @@ public void initialize() { @Override public void execute() { if (launcher.readyToLaunch(launchVelo)) { - index.feed(); + if (launchVelo > 0) { + index.feed(); + } else { + index.reverseFeed(); + } } } From 54cf2fdd334adfba7bfa0ccce616a22b8686e995 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Sat, 2 Mar 2024 09:36:37 -0500 Subject: [PATCH 05/30] stop lower belt on end of index --- src/main/java/frc/robot/commands/PrimeIndex.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/commands/PrimeIndex.java b/src/main/java/frc/robot/commands/PrimeIndex.java index b1b561f1..a32c7886 100644 --- a/src/main/java/frc/robot/commands/PrimeIndex.java +++ b/src/main/java/frc/robot/commands/PrimeIndex.java @@ -49,6 +49,7 @@ public void execute() { @Override public void end(boolean interrupted) { index.stop(); + transfer.stop(); } // Returns true when the command should end. From a042f4a0232b367d851e97fc6f83d93b45a5f338 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Sat, 2 Mar 2024 09:36:57 -0500 Subject: [PATCH 06/30] Add transfer to intake --- src/main/java/frc/robot/commands/ToggleIntake.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/ToggleIntake.java b/src/main/java/frc/robot/commands/ToggleIntake.java index f3a8d0a0..ad138280 100644 --- a/src/main/java/frc/robot/commands/ToggleIntake.java +++ b/src/main/java/frc/robot/commands/ToggleIntake.java @@ -24,7 +24,11 @@ public ToggleIntake(Intake intake, Transfer transfer) { public void initialize() { System.out.println("Intake is initializing"); intake.toggle(); - transfer.toggle(); + if (intake.isRunning()){ + transfer.start(); + } else { + transfer.stop(); + } } // Returns true when the command should end. From 3515d8f965dbd6b9925bb109a4c73d2f3a44dd13 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Sat, 2 Mar 2024 09:37:21 -0500 Subject: [PATCH 07/30] Speed and button binding changes --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 25 ++++++++++++++++----- 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c60f38f2..633743cf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -92,7 +92,7 @@ public static class Climb { // These are break beam sensor IDS public static final int winchLimitLeft = 2; public static final int winchLimitRight = 3; - public static final double motorSpeedFactor = -0.5; + public static final double motorSpeedFactor = -0.9; } public static class Transfer { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 11970555..7d4d6281 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -61,7 +61,7 @@ public RobotContainer() { NamedCommands.registerCommand("intake", new ToggleIntake(m_intake, m_transfer)); NamedCommands.registerCommand("index", new PrimeIndex(m_index, m_transfer)); NamedCommands.registerCommand( - "align-launch", new AlignLaunchAuto(m_swerve, m_launch, m_index, 3000, 1)); + "align-launch", new AlignLaunchAuto(m_swerve, m_launch, m_index, 5300, 1)); NamedCommands.registerCommand("reverse intake", m_intake.reverseIntakeCommand()); CameraServer.startAutomaticCapture(); @@ -94,11 +94,13 @@ public RobotContainer() { 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("2 Centerline", m_swerve.getAutonomousCommand("2 Centerline")); - m_chooser.addOption("3 Centerline", m_swerve.getAutonomousCommand("3 Centerline")); - m_chooser.addOption("4 Centerline", m_swerve.getAutonomousCommand("4 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 Chill", m_swerve.noAuto()); + m_chooser.addOption("Simple Shoot Center", m_swerve.getAutonomousCommand("Simple Shoot Center")); + m_chooser.addOption("Simple Shoot Safe", m_swerve.getAutonomousCommand("Simple Shoot Safe")); + m_chooser.addOption("Simple Shoot Amp", m_swerve.getAutonomousCommand("Simple Shoot Amp")); + SmartDashboard.putData(m_chooser); } @@ -117,6 +119,9 @@ private void configureBindings() { JoystickButton leftBumper = new JoystickButton(driver, XboxController.Button.kLeftBumper.value); leftBumper.onTrue(new ToggleIntake(m_intake, m_transfer)); + JoystickButton rb = new JoystickButton(driver, XboxController.Button.kRightBumper.value); + rb.whileTrue(new LaunchWithVelo(m_launch, m_index, -1500, false)); + JoystickButton coRB = new JoystickButton(coDriver, XboxController.Button.kRightBumper.value); coRB.onTrue(new PrimeIndex(m_index, m_transfer)); @@ -124,13 +129,19 @@ private void configureBindings() { lt.whileTrue(m_swerve.alignCommand()); Trigger rt = new Trigger(() -> driver.getRightTriggerAxis() >= 0.05); - rt.whileTrue(new LaunchWithVelo(m_launch, m_index, 2500, false)); + rt.whileTrue(new LaunchWithVelo(m_launch, m_index, 5200, false)); JoystickButton x = new JoystickButton(driver, XboxController.Button.kX.value); x.onTrue(m_swerve.updatePositionCommand()); + JoystickButton a = new JoystickButton(driver, XboxController.Button.kA.value); + a.whileTrue(new LaunchWithVelo(m_launch, m_index, 2050, false)); + JoystickButton y = new JoystickButton(driver, XboxController.Button.kY.value); y.whileTrue(m_intake.reverseIntakeCommand()); + + JoystickButton b = new JoystickButton(driver, XboxController.Button.kB.value); + b.whileTrue(m_swerve.setRotationCommand(180)); } /** @@ -162,4 +173,8 @@ public void updatePose() { public void dropLauncher() { m_launch.releaseLauncher(); } + + public void lockLauncher(){ + m_launch.lockLauncher(); + } } From 5d24658f36292bbe7c4684588c5bb9c36e91e97c Mon Sep 17 00:00:00 2001 From: robo7660 Date: Sat, 2 Mar 2024 09:37:38 -0500 Subject: [PATCH 08/30] add lock launcher at teleop and robot init --- src/main/java/frc/robot/Robot.java | 3 ++- src/main/java/frc/robot/subsystems/Launcher.java | 4 ++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ca2b6bec..99a6a76b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -34,6 +34,7 @@ public void robotInit() { // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); getAlliance(); + m_robotContainer.lockLauncher(); } private void getAlliance() { @@ -95,7 +96,7 @@ public void teleopInit() { } getAlliance(); m_robotContainer.updatePose(); - m_robotContainer.dropLauncher(); + 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 df78a992..b94ec3d0 100644 --- a/src/main/java/frc/robot/subsystems/Launcher.java +++ b/src/main/java/frc/robot/subsystems/Launcher.java @@ -210,6 +210,10 @@ public void releaseLauncher() { angle.setIdleMode(IdleMode.kCoast); } + public void lockLauncher() { + angle.setIdleMode(IdleMode.kBrake); + } + public Command switchAngleCommand() { return this.runOnce(() -> switchAngle()); } From 5ce71afc73b73b0064de3a3a19ac83818f096ed1 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Sat, 2 Mar 2024 09:37:52 -0500 Subject: [PATCH 09/30] Reverse index --- src/main/java/frc/robot/subsystems/Index.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Index.java b/src/main/java/frc/robot/subsystems/Index.java index f925b575..ee2c5064 100644 --- a/src/main/java/frc/robot/subsystems/Index.java +++ b/src/main/java/frc/robot/subsystems/Index.java @@ -42,6 +42,10 @@ public void feed() { motorUpper.set(0.9); } + public void reverseFeed() { + motorUpper.set(-0.9); + } + public void setUpper(double speed) { motorUpper.set(speed); } From 4471b53fb47254981d32add9e2018693f091b8f2 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Sat, 2 Mar 2024 09:38:05 -0500 Subject: [PATCH 10/30] tuning for swerve in matches --- .../java/frc/robot/subsystems/SwerveSubsystem.java | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index b8c854cd..2cc310d5 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -52,7 +52,7 @@ public class SwerveSubsystem extends SubsystemBase /** * Maximum speed of the robot in meters per second, used to limit acceleration. */ - public double maximumSpeed = Units.feetToMeters(14.5); + public double maximumSpeed = Units.feetToMeters(30); /** * Initialize {@link SwerveDrive} with the directory provided. @@ -207,7 +207,7 @@ public Command driveToPose(Pose2d pose) public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX, DoubleSupplier headingY) { - // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control. + swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control. return run(() -> { double xInput = Math.pow(translationX.getAsDouble(), 3); // Smooth controll out double yInput = Math.pow(translationY.getAsDouble(), 3); // Smooth controll out @@ -230,7 +230,7 @@ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translat */ public Command simDriveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier rotation) { - // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control. + swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control. return run(() -> { // Make the robot move driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(translationX.getAsDouble(), @@ -649,4 +649,12 @@ public Command zeroGyroCommand(){ public Command setRotationCommand(double rotation){ return this.run(() -> setRotation(Units.degreesToRadians(rotation))); } + + public void nullFunction() { + return; + } + + public Command noAuto() { + return this.runOnce(() -> nullFunction()); + } } From 1e18b3c966128bc276dee6ba78108ca4f6ef70fb Mon Sep 17 00:00:00 2001 From: robo7660 Date: Sat, 2 Mar 2024 19:53:12 -0500 Subject: [PATCH 11/30] Fix safe shoot paths to account for weird rotation --- .../autos/Simple Shoot Safe Blue.auto | 37 +++++++++++++ ...t Safe.auto => Simple Shoot Safe Red.auto} | 6 +-- .../paths/Simple Shoot Center 1.path | 6 +-- ...e 1.path => Simple Shoot Safe 1 Blue.path} | 18 +++---- .../paths/Simple Shoot Safe 1 Red.path | 52 +++++++++++++++++++ .../paths/Simple Shoot Safe 2.path | 18 +++---- 6 files changed, 113 insertions(+), 24 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Simple Shoot Safe Blue.auto rename src/main/deploy/pathplanner/autos/{Simple Shoot Safe.auto => Simple Shoot Safe Red.auto} (83%) rename src/main/deploy/pathplanner/paths/{Simple Shoot Safe 1.path => Simple Shoot Safe 1 Blue.path} (73%) create mode 100644 src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Red.path diff --git a/src/main/deploy/pathplanner/autos/Simple Shoot Safe Blue.auto b/src/main/deploy/pathplanner/autos/Simple Shoot Safe Blue.auto new file mode 100644 index 00000000..391509d9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Simple Shoot Safe Blue.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.77430354290908, + "y": 4.303448834817898 + }, + "rotation": 120.48894049983096 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Simple Shoot Safe 1 Blue" + } + }, + { + "type": "named", + "data": { + "name": "align-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/autos/Simple Shoot Safe.auto b/src/main/deploy/pathplanner/autos/Simple Shoot Safe Red.auto similarity index 83% rename from src/main/deploy/pathplanner/autos/Simple Shoot Safe.auto rename to src/main/deploy/pathplanner/autos/Simple Shoot Safe Red.auto index 5ad177e5..f56c1571 100644 --- a/src/main/deploy/pathplanner/autos/Simple Shoot Safe.auto +++ b/src/main/deploy/pathplanner/autos/Simple Shoot Safe Red.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7336934388833163, - "y": 4.406890714018958 + "x": 0.77430354290908, + "y": 4.303448834817898 }, "rotation": 120.48894049983096 }, @@ -14,7 +14,7 @@ { "type": "path", "data": { - "pathName": "Simple Shoot Safe 1" + "pathName": "Simple Shoot Safe 1 Red" } }, { diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Center 1.path b/src/main/deploy/pathplanner/paths/Simple Shoot Center 1.path index b3802ec7..cef18247 100644 --- a/src/main/deploy/pathplanner/paths/Simple Shoot Center 1.path +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Center 1.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 3.2588808170490466, + "x": 3.902364859361549, "y": 5.547612425391122 }, "prevControl": { - "x": 2.2588808170490466, + "x": 2.902364859361549, "y": 5.547612425391122 }, "nextControl": null, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 178.8982938847937, + "rotation": -160.34617594194665, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1.path b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Blue.path similarity index 73% rename from src/main/deploy/pathplanner/paths/Simple Shoot Safe 1.path rename to src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Blue.path index 33e2214a..c66cd3c5 100644 --- a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1.path +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Blue.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.7336934388833163, - "y": 4.406890714018958 + "x": 0.77430354290908, + "y": 4.303448834817898 }, "prevControl": null, "nextControl": { - "x": 0.7336934388833163, - "y": 4.37764143936839 + "x": 0.77430354290908, + "y": 4.27419956016733 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.7184190187857824, - "y": 2.856679157538838 + "x": 2.8688904883748028, + "y": 3.1491719040445205 }, "prevControl": { - "x": 1.689169744135214, - "y": 2.895678190406262 + "x": 2.8396412137242346, + "y": 3.1881709369119444 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 99.1301764822787, + "rotation": 93.75172907052597, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Red.path b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Red.path new file mode 100644 index 00000000..0edf40da --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Red.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.77430354290908, + "y": 4.303448834817898 + }, + "prevControl": null, + "nextControl": { + "x": 0.77430354290908, + "y": 4.27419956016733 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8688904883748028, + "y": 3.1491719040445205 + }, + "prevControl": { + "x": 2.8396412137242346, + "y": 3.1881709369119444 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 165.7565394264522, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 122.23888483747, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file 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 b28165ef..ddb6d142 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": 1.72, - "y": 2.86 + "x": 2.58, + "y": 3.02 }, "prevControl": null, "nextControl": { - "x": 2.7199999999999998, - "y": 2.8599999999999994 + "x": 2.63367409925703, + "y": 1.7073465485962325 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.266031444176903, - "y": 1.1309719531553086 + "x": 6.925428779632488, + "y": 0.6665668032190769 }, "prevControl": { - "x": 6.266031444176903, - "y": 1.1309719531553086 + "x": 6.200391204201211, + "y": 0.7016492665463968 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "reversed": false, "folder": null, "previewStartingState": { - "rotation": -58.37925702479779, + "rotation": 134.26332354707648, "velocity": 0 }, "useDefaultConstraints": true From 65bbbcc97c5f97324339af80e6b481983101869b Mon Sep 17 00:00:00 2001 From: robo7660 Date: Sat, 2 Mar 2024 19:53:31 -0500 Subject: [PATCH 12/30] fix update position to clear infinite loop --- src/main/java/frc/robot/RobotContainer.java | 8 +++++--- src/main/java/frc/robot/subsystems/SwerveSubsystem.java | 9 +++++---- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7d4d6281..f7d16cf1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -64,7 +64,8 @@ public RobotContainer() { "align-launch", new AlignLaunchAuto(m_swerve, m_launch, m_index, 5300, 1)); NamedCommands.registerCommand("reverse intake", m_intake.reverseIntakeCommand()); - CameraServer.startAutomaticCapture(); + CameraServer.startAutomaticCapture(0); + CameraServer.startAutomaticCapture(1); // Configure the trigger bindings configureBindings(); @@ -98,8 +99,9 @@ public RobotContainer() { m_chooser.addOption("Close 2", m_swerve.getAutonomousCommand("Close 2")); m_chooser.addOption("Just Chill", m_swerve.noAuto()); m_chooser.addOption("Simple Shoot Center", m_swerve.getAutonomousCommand("Simple Shoot Center")); - m_chooser.addOption("Simple Shoot Safe", m_swerve.getAutonomousCommand("Simple Shoot Safe")); + 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")); SmartDashboard.putData(m_chooser); @@ -132,7 +134,7 @@ private void configureBindings() { rt.whileTrue(new LaunchWithVelo(m_launch, m_index, 5200, false)); JoystickButton x = new JoystickButton(driver, XboxController.Button.kX.value); - x.onTrue(m_swerve.updatePositionCommand()); + x.whileTrue(m_swerve.updatePositionCommand()); JoystickButton a = new JoystickButton(driver, XboxController.Button.kA.value); a.whileTrue(new LaunchWithVelo(m_launch, m_index, 2050, false)); diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 2cc310d5..12de35ec 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -543,10 +543,11 @@ public void setRotation(double angle) { swerveDrive.drive(desired); } - public void resetToLimelight() { + public void + resetToLimelight() { boolean hasTarget = LimelightHelpers.getTV(Constants.limelightName); - while (!hasTarget) { - hasTarget = LimelightHelpers.getTV(Constants.limelightName); + if (!hasTarget) { + return; } Pose2d pose; if (Robot.alliance == Alliance.Red && hasTarget) { @@ -639,7 +640,7 @@ public Command alignCommand() { } public Command updatePositionCommand() { - return this.runOnce(() -> resetToLimelight()); + return this.run(() -> resetToLimelight()); } public Command zeroGyroCommand(){ From bc7ae172f81e3b5d0116b908e9c5975b5fa50a60 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Sat, 2 Mar 2024 19:59:16 -0500 Subject: [PATCH 13/30] Add dashboard to repo --- Dashboards/MILFORD_DASH.html | 67 ++++++++++++++++++++++++++++++++++++ 1 file changed, 67 insertions(+) create mode 100644 Dashboards/MILFORD_DASH.html diff --git a/Dashboards/MILFORD_DASH.html b/Dashboards/MILFORD_DASH.html new file mode 100644 index 00000000..cfd6d8b2 --- /dev/null +++ b/Dashboards/MILFORD_DASH.html @@ -0,0 +1,67 @@ + \ No newline at end of file From 23a54bdec34c68c7b3a465c5ba924e56aba4c830 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Sat, 2 Mar 2024 20:06:32 -0500 Subject: [PATCH 14/30] MILFORD COMP WHITESPACE --- src/main/java/frc/robot/RobotContainer.java | 12 +++++++----- src/main/java/frc/robot/commands/ToggleIntake.java | 2 +- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f7d16cf1..c2957de1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -98,11 +98,13 @@ public RobotContainer() { m_chooser.addOption("5 Centerline", m_swerve.getAutonomousCommand("5 Centerline")); m_chooser.addOption("Close 2", m_swerve.getAutonomousCommand("Close 2")); m_chooser.addOption("Just Chill", m_swerve.noAuto()); - m_chooser.addOption("Simple Shoot Center", m_swerve.getAutonomousCommand("Simple Shoot Center")); - m_chooser.addOption("Simple Shoot Safe Blue", m_swerve.getAutonomousCommand("Simple Shoot Safe Blue")); + m_chooser.addOption( + "Simple Shoot Center", m_swerve.getAutonomousCommand("Simple Shoot 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")); - + m_chooser.addOption( + "Simple Shoot Safe Red", m_swerve.getAutonomousCommand("Simple Shoot Safe Red")); SmartDashboard.putData(m_chooser); } @@ -176,7 +178,7 @@ public void dropLauncher() { m_launch.releaseLauncher(); } - public void lockLauncher(){ + public void lockLauncher() { m_launch.lockLauncher(); } } diff --git a/src/main/java/frc/robot/commands/ToggleIntake.java b/src/main/java/frc/robot/commands/ToggleIntake.java index ad138280..f8de9ace 100644 --- a/src/main/java/frc/robot/commands/ToggleIntake.java +++ b/src/main/java/frc/robot/commands/ToggleIntake.java @@ -24,7 +24,7 @@ public ToggleIntake(Intake intake, Transfer transfer) { public void initialize() { System.out.println("Intake is initializing"); intake.toggle(); - if (intake.isRunning()){ + if (intake.isRunning()) { transfer.start(); } else { transfer.stop(); From d1b541e1da04006233e6c67f122be6f89d30c8ba Mon Sep 17 00:00:00 2001 From: robo7660 Date: Tue, 5 Mar 2024 20:32:10 -0500 Subject: [PATCH 15/30] Chaged Pathplanner max velo --- .pathplanner/settings.json | 2 +- src/main/deploy/pathplanner/paths/CenterLine 1.path | 2 +- src/main/deploy/pathplanner/paths/Centerline 1-2.path | 2 +- src/main/deploy/pathplanner/paths/Centerline 1-3.path | 2 +- src/main/deploy/pathplanner/paths/Centerline 1-4.path | 2 +- src/main/deploy/pathplanner/paths/Centerline 1-5.path | 2 +- src/main/deploy/pathplanner/paths/Close 2-1.path | 2 +- src/main/deploy/pathplanner/paths/Close 2-2.path | 2 +- src/main/deploy/pathplanner/paths/Close 2-3.path | 2 +- src/main/deploy/pathplanner/paths/Close 2-Escape.path | 2 +- src/main/deploy/pathplanner/paths/Mid 2 Piece 1.path | 2 +- src/main/deploy/pathplanner/paths/Mid 2 Piece 2.path | 2 +- src/main/deploy/pathplanner/paths/Mid 2 Piece 3.path | 2 +- src/main/deploy/pathplanner/paths/Rotate 90.path | 2 +- src/main/deploy/pathplanner/paths/Simple Shoot Amp 1.path | 2 +- src/main/deploy/pathplanner/paths/Simple Shoot Amp 2.path | 2 +- src/main/deploy/pathplanner/paths/Simple Shoot Amp 3.path | 2 +- src/main/deploy/pathplanner/paths/Simple Shoot Center 1.path | 2 +- src/main/deploy/pathplanner/paths/Simple Shoot Center 2.path | 2 +- .../deploy/pathplanner/paths/Simple Shoot Safe 1 Blue.path | 4 ++-- .../deploy/pathplanner/paths/Simple Shoot Safe 1 Red.path | 4 ++-- src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path | 2 +- src/main/deploy/pathplanner/paths/drive 1m.path | 2 +- 23 files changed, 25 insertions(+), 25 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 6d89aa88..bc3a84c9 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -6,7 +6,7 @@ "New Folder" ], "autoFolders": [], - "defaultMaxVel": 2.0, + "defaultMaxVel": 15.0, "defaultMaxAccel": 5.0, "defaultMaxAngVel": 180.0, "defaultMaxAngAccel": 180.0, diff --git a/src/main/deploy/pathplanner/paths/CenterLine 1.path b/src/main/deploy/pathplanner/paths/CenterLine 1.path index ced121b8..12e7dacd 100644 --- a/src/main/deploy/pathplanner/paths/CenterLine 1.path +++ b/src/main/deploy/pathplanner/paths/CenterLine 1.path @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Centerline 1-2.path b/src/main/deploy/pathplanner/paths/Centerline 1-2.path index 1251dd60..1f7be50f 100644 --- a/src/main/deploy/pathplanner/paths/Centerline 1-2.path +++ b/src/main/deploy/pathplanner/paths/Centerline 1-2.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Centerline 1-3.path b/src/main/deploy/pathplanner/paths/Centerline 1-3.path index a2afe3ba..435c51f3 100644 --- a/src/main/deploy/pathplanner/paths/Centerline 1-3.path +++ b/src/main/deploy/pathplanner/paths/Centerline 1-3.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Centerline 1-4.path b/src/main/deploy/pathplanner/paths/Centerline 1-4.path index 838acb79..3b06e9d7 100644 --- a/src/main/deploy/pathplanner/paths/Centerline 1-4.path +++ b/src/main/deploy/pathplanner/paths/Centerline 1-4.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Centerline 1-5.path b/src/main/deploy/pathplanner/paths/Centerline 1-5.path index 288f26f8..52d58695 100644 --- a/src/main/deploy/pathplanner/paths/Centerline 1-5.path +++ b/src/main/deploy/pathplanner/paths/Centerline 1-5.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Close 2-1.path b/src/main/deploy/pathplanner/paths/Close 2-1.path index b3da8a35..5eaa51c0 100644 --- a/src/main/deploy/pathplanner/paths/Close 2-1.path +++ b/src/main/deploy/pathplanner/paths/Close 2-1.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Close 2-2.path b/src/main/deploy/pathplanner/paths/Close 2-2.path index e0331722..d79f4a61 100644 --- a/src/main/deploy/pathplanner/paths/Close 2-2.path +++ b/src/main/deploy/pathplanner/paths/Close 2-2.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Close 2-3.path b/src/main/deploy/pathplanner/paths/Close 2-3.path index 114fba54..8463c654 100644 --- a/src/main/deploy/pathplanner/paths/Close 2-3.path +++ b/src/main/deploy/pathplanner/paths/Close 2-3.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Close 2-Escape.path b/src/main/deploy/pathplanner/paths/Close 2-Escape.path index c3700968..1a135d72 100644 --- a/src/main/deploy/pathplanner/paths/Close 2-Escape.path +++ b/src/main/deploy/pathplanner/paths/Close 2-Escape.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Mid 2 Piece 1.path b/src/main/deploy/pathplanner/paths/Mid 2 Piece 1.path index e60a2102..63a7af51 100644 --- a/src/main/deploy/pathplanner/paths/Mid 2 Piece 1.path +++ b/src/main/deploy/pathplanner/paths/Mid 2 Piece 1.path @@ -38,7 +38,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Mid 2 Piece 2.path b/src/main/deploy/pathplanner/paths/Mid 2 Piece 2.path index ca9864c5..c3298d79 100644 --- a/src/main/deploy/pathplanner/paths/Mid 2 Piece 2.path +++ b/src/main/deploy/pathplanner/paths/Mid 2 Piece 2.path @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Mid 2 Piece 3.path b/src/main/deploy/pathplanner/paths/Mid 2 Piece 3.path index 3a134236..f74adcdc 100644 --- a/src/main/deploy/pathplanner/paths/Mid 2 Piece 3.path +++ b/src/main/deploy/pathplanner/paths/Mid 2 Piece 3.path @@ -38,7 +38,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Rotate 90.path b/src/main/deploy/pathplanner/paths/Rotate 90.path index c36eceda..7dbceff8 100644 --- a/src/main/deploy/pathplanner/paths/Rotate 90.path +++ b/src/main/deploy/pathplanner/paths/Rotate 90.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Amp 1.path b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 1.path index c80da93f..72b471fe 100644 --- a/src/main/deploy/pathplanner/paths/Simple Shoot Amp 1.path +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 1.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Amp 2.path b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 2.path index 20a6dd98..5c78a5d8 100644 --- a/src/main/deploy/pathplanner/paths/Simple Shoot Amp 2.path +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 2.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 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 0945df8e..670c8d98 100644 --- a/src/main/deploy/pathplanner/paths/Simple Shoot Amp 3.path +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 3.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Center 1.path b/src/main/deploy/pathplanner/paths/Simple Shoot Center 1.path index cef18247..ae44b6d8 100644 --- a/src/main/deploy/pathplanner/paths/Simple Shoot Center 1.path +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Center 1.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 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 c820da0e..27f7c353 100644 --- a/src/main/deploy/pathplanner/paths/Simple Shoot Center 2.path +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Center 2.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Blue.path b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Blue.path index c66cd3c5..0990d7e5 100644 --- a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Blue.path +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Blue.path @@ -32,14 +32,14 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, - "rotation": 93.75172907052597, + "rotation": 137.4528248632394, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Red.path b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Red.path index 0edf40da..360ce985 100644 --- a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Red.path +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Red.path @@ -32,14 +32,14 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, - "rotation": 165.7565394264522, + "rotation": 141.52214025945182, "rotateFast": false }, "reversed": 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 ddb6d142..ffb2c456 100644 --- a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/drive 1m.path b/src/main/deploy/pathplanner/paths/drive 1m.path index a4204a30..a82d9acc 100644 --- a/src/main/deploy/pathplanner/paths/drive 1m.path +++ b/src/main/deploy/pathplanner/paths/drive 1m.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 From 3dcd0d3b62c54d5b56996c8a7b480eda2c097bf9 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Tue, 5 Mar 2024 20:32:31 -0500 Subject: [PATCH 16/30] Change messed up encoder 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 8d6a03c3..ce04c2e7 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": 180, + "absoluteEncoderOffset": 283.71, "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 a58ed1b5..1a338108 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": 125, + "absoluteEncoderOffset": 0, "location": { "front": 13, "left": -13 From 453bc09d8ace60266f912c5a8d8052bcff988301 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Tue, 5 Mar 2024 20:32:59 -0500 Subject: [PATCH 17/30] Increase acceleration of bot --- .../swerves/KrakenSwerve/modules/physicalproperties.json | 8 ++++---- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/subsystems/SwerveSubsystem.java | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json b/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json index d32da52b..1457155b 100644 --- a/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json +++ b/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json @@ -4,12 +4,12 @@ "angle": 16.8 }, "currentLimit": { - "drive": 40, - "angle": 20 + "drive": 70, + "angle": 60 }, "rampRate": { - "drive": 0.25, - "angle": 0.25 + "drive": 0.05, + "angle": 0.05 }, "wheelGripCoefficientOfFriction": 1.19, "optimalVoltage": 12 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 633743cf..502c1fe4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,9 +25,9 @@ public final class Constants { public static final boolean kRightFrontDriveInverted = false; public static final double LOOP_TIME = 0.13; - public static final double ROBOT_MASS = 150 * 0.453592; + public static final double ROBOT_MASS = 115 * 0.453592; public static final Matter CHASSIS = - new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); + new Matter(new Translation3d(0, 0, Units.inchesToMeters(0)), ROBOT_MASS); public static final PIDConstants autoTranslationPID = new PIDConstants(0.7, 0, 0); public static final PIDConstants autoRotationPID = new PIDConstants(0.4, 0, 0.01); diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 12de35ec..ebf051bb 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -86,7 +86,7 @@ public SwerveSubsystem(File directory) { throw new RuntimeException(e); } - swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle. + swerveDrive.setHeadingCorrection(true); // Heading correction should only be used while controlling the robot via angle. swerveDrive.setCosineCompensator(!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life. setupPathPlanner(); From 0e9642a553573855ee8e9926a0f2e0acf810e206 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Tue, 5 Mar 2024 20:39:44 -0500 Subject: [PATCH 18/30] another encoder fix --- src/main/deploy/swerves/KrakenSwerve/modules/frontright.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/frontright.json b/src/main/deploy/swerves/KrakenSwerve/modules/frontright.json index 1a338108..0db6e74b 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": 0, + "absoluteEncoderOffset": 18, "location": { "front": 13, "left": -13 From e4bc50e1474dbcfac5a6ffeb88cc6cb0c2a6b82d Mon Sep 17 00:00:00 2001 From: robo7660 Date: Wed, 6 Mar 2024 11:55:31 -0500 Subject: [PATCH 19/30] Add YAGSL Example Absolute Drive Advanced --- src/main/java/frc/robot/Constants.java | 1 + .../frc/robot/commands/AbsoluteDriveAdv.java | 154 ++++++++++++++++++ 2 files changed, 155 insertions(+) create mode 100644 src/main/java/frc/robot/commands/AbsoluteDriveAdv.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 502c1fe4..b89741d5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -108,5 +108,6 @@ public static class OperatorConstants { 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 int TURN_CONSTANT = 6; } } diff --git a/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java new file mode 100644 index 00000000..32ce3cce --- /dev/null +++ b/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java @@ -0,0 +1,154 @@ +// 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.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.SwerveSubsystem; +import java.util.List; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import swervelib.SwerveController; +import swervelib.math.SwerveMath; + +/** + * A more advanced Swerve Control System that has 4 buttons for which direction to face + */ +public class AbsoluteDriveAdv extends Command +{ + + private final SwerveSubsystem swerve; + private final DoubleSupplier vX, vY; + private final DoubleSupplier headingAdjust; + private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; + private boolean resetHeading = false; + + /** + * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is + * torwards/away from alliance wall and y is left/right. Heading Adjust changes the current heading after being + * multipied by a constant. The look booleans are shortcuts to get the robot to face a certian direction. Based off of + * ideas in https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172 + * + * @param swerve The swerve drivebase subsystem. + * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range -1 to 1 + * with deadband already accounted for. Positive X is away from the alliance wall. + * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range -1 to 1 + * with deadband already accounted for. Positive Y is towards the left wall when looking through + * the driver station glass. + * @param headingAdjust DoubleSupplier that supplies the component of the robot's heading angle that should be + * adjusted. Should range from -1 to 1 with deadband already accounted for. + * @param lookAway Face the robot towards the opposing alliance's wall in the same direction the driver is + * facing + * @param lookTowards Face the robot towards the driver + * @param lookLeft Face the robot left + * @param lookRight Face the robot right + */ + public AbsoluteDriveAdv(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingAdjust, + BooleanSupplier lookAway, BooleanSupplier lookTowards, BooleanSupplier lookLeft, + BooleanSupplier lookRight) + { + this.swerve = swerve; + this.vX = vX; + this.vY = vY; + this.headingAdjust = headingAdjust; + this.lookAway = lookAway; + this.lookTowards = lookTowards; + this.lookLeft = lookLeft; + this.lookRight = lookRight; + + addRequirements(swerve); + } + + @Override + public void initialize() + { + resetHeading = true; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() + { + double headingX = 0; + double headingY = 0; + + // These are written to allow combinations for 45 angles + // Face Away from Drivers + if (lookAway.getAsBoolean()) + { + headingY = -1; + } + // Face Right + if (lookRight.getAsBoolean()) + { + headingX = 1; + } + // Face Left + if (lookLeft.getAsBoolean()) + { + headingX = -1; + } + // Face Towards the Drivers + if (lookTowards.getAsBoolean()) + { + headingY = 1; + } + + // Prevent Movement After Auto + if (resetHeading) + { + if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) + { + // Get the curret Heading + Rotation2d currentHeading = swerve.getHeading(); + + // Set the Current Heading to the desired Heading + headingX = currentHeading.getSin(); + headingY = currentHeading.getCos(); + } + //Dont reset Heading Again + resetHeading = false; + } + + ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY); + + // Limit velocity to prevent tippy + Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); + translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(), + Constants.LOOP_TIME, Constants.ROBOT_MASS, List.of(Constants.CHASSIS), + swerve.getSwerveDriveConfiguration()); + SmartDashboard.putNumber("LimitedTranslation", translation.getX()); + SmartDashboard.putString("Translation", translation.toString()); + + // Make the robot move + if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) + { + resetHeading = true; + swerve.drive(translation, (Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), true); + } else + { + swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + } + } + + // 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; + } + + +} \ No newline at end of file From 9daa0a5f0767159f5771fce2d8e6bb2129b094b5 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Wed, 6 Mar 2024 11:56:01 -0500 Subject: [PATCH 20/30] Some fixes suggested by rat pack --- .../deploy/swerves/KrakenSwerve/modules/physicalproperties.json | 2 +- .../deploy/swerves/KrakenSwerve/modules/pidfproperties.json | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json b/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json index 1457155b..c192b5bf 100644 --- a/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json +++ b/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json @@ -12,5 +12,5 @@ "angle": 0.05 }, "wheelGripCoefficientOfFriction": 1.19, - "optimalVoltage": 12 + "optimalVoltage": 10.5 } \ No newline at end of file diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/pidfproperties.json b/src/main/deploy/swerves/KrakenSwerve/modules/pidfproperties.json index 3d4d5c55..e032133b 100644 --- a/src/main/deploy/swerves/KrakenSwerve/modules/pidfproperties.json +++ b/src/main/deploy/swerves/KrakenSwerve/modules/pidfproperties.json @@ -1,7 +1,7 @@ { "drive": { "p": 1, - "i": 0, + "i": 0.01, "d": 0, "f": 0, "iz": 0 From 868391e391d2cea55dd00ab39ffb0ea550526958 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Wed, 6 Mar 2024 11:56:22 -0500 Subject: [PATCH 21/30] more encoder updates...will make more precise later --- src/main/deploy/swerves/KrakenSwerve/modules/frontleft.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/frontleft.json b/src/main/deploy/swerves/KrakenSwerve/modules/frontleft.json index ce04c2e7..fef30ebd 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": 283.71, + "absoluteEncoderOffset": 330, "location": { "front": 13, "left": 13 From 49e3b4a8f9473f6a6f3a6fd59c0a69faf9539aa4 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Wed, 6 Mar 2024 12:07:08 -0500 Subject: [PATCH 22/30] Impliment advanced drive --- src/main/java/frc/robot/RobotContainer.java | 21 ++- .../frc/robot/commands/AbsoluteDriveAdv.java | 124 +++++++++--------- 2 files changed, 80 insertions(+), 65 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c2957de1..3977b093 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,9 +15,11 @@ 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.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; @@ -70,6 +72,11 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); + POVButton driverUp = new POVButton(driver, 0); + POVButton driverLeft = new POVButton(driver, 90); + POVButton driverDown = new POVButton(driver, 180); + POVButton driverRight = new POVButton(driver, 270); + AbsoluteDrive closedAbsoluteDrive = new AbsoluteDrive( m_swerve, @@ -81,8 +88,18 @@ public RobotContainer() { () -> -driver.getRightX(), () -> -driver.getRightY()); - m_swerve.setDefaultCommand( - !RobotBase.isSimulation() ? closedAbsoluteDrive : closedAbsoluteDrive); + AbsoluteDriveAdv advancedDrive = + new AbsoluteDriveAdv( + 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), + () -> driverUp.getAsBoolean(), + () -> driverDown.getAsBoolean(), + () -> driverLeft.getAsBoolean(), + () -> driverRight.getAsBoolean()); + + m_swerve.setDefaultCommand(!RobotBase.isSimulation() ? advancedDrive : advancedDrive); // -m_index.setDefaultCommand(m_index.manualIntake(coDriver::getRightY)); diff --git a/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java index 32ce3cce..d035103b 100644 --- a/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java +++ b/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java @@ -17,42 +17,45 @@ import swervelib.SwerveController; import swervelib.math.SwerveMath; -/** - * A more advanced Swerve Control System that has 4 buttons for which direction to face - */ -public class AbsoluteDriveAdv extends Command -{ +/** A more advanced Swerve Control System that has 4 buttons for which direction to face */ +public class AbsoluteDriveAdv extends Command { private final SwerveSubsystem swerve; - private final DoubleSupplier vX, vY; - private final DoubleSupplier headingAdjust; + private final DoubleSupplier vX, vY; + private final DoubleSupplier headingAdjust; private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; - private boolean resetHeading = false; + private boolean resetHeading = false; /** - * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is - * torwards/away from alliance wall and y is left/right. Heading Adjust changes the current heading after being - * multipied by a constant. The look booleans are shortcuts to get the robot to face a certian direction. Based off of - * ideas in https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172 + * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, + * where x is torwards/away from alliance wall and y is left/right. Heading Adjust changes the + * current heading after being multipied by a constant. The look booleans are shortcuts to get the + * robot to face a certian direction. Based off of ideas in + * https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172 * - * @param swerve The swerve drivebase subsystem. - * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range -1 to 1 - * with deadband already accounted for. Positive X is away from the alliance wall. - * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range -1 to 1 - * with deadband already accounted for. Positive Y is towards the left wall when looking through - * the driver station glass. - * @param headingAdjust DoubleSupplier that supplies the component of the robot's heading angle that should be - * adjusted. Should range from -1 to 1 with deadband already accounted for. - * @param lookAway Face the robot towards the opposing alliance's wall in the same direction the driver is - * facing - * @param lookTowards Face the robot towards the driver - * @param lookLeft Face the robot left - * @param lookRight Face the robot right + * @param swerve The swerve drivebase subsystem. + * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. + * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when + * looking through the driver station glass. + * @param headingAdjust DoubleSupplier that supplies the component of the robot's heading angle + * that should be adjusted. Should range from -1 to 1 with deadband already accounted for. + * @param lookAway Face the robot towards the opposing alliance's wall in the same direction the + * driver is facing + * @param lookTowards Face the robot towards the driver + * @param lookLeft Face the robot left + * @param lookRight Face the robot right */ - public AbsoluteDriveAdv(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingAdjust, - BooleanSupplier lookAway, BooleanSupplier lookTowards, BooleanSupplier lookLeft, - BooleanSupplier lookRight) - { + public AbsoluteDriveAdv( + SwerveSubsystem swerve, + DoubleSupplier vX, + DoubleSupplier vY, + DoubleSupplier headingAdjust, + BooleanSupplier lookAway, + BooleanSupplier lookTowards, + BooleanSupplier lookLeft, + BooleanSupplier lookRight) { this.swerve = swerve; this.vX = vX; this.vY = vY; @@ -66,45 +69,37 @@ public AbsoluteDriveAdv(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplie } @Override - public void initialize() - { + public void initialize() { resetHeading = true; } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() - { + public void execute() { double headingX = 0; double headingY = 0; // These are written to allow combinations for 45 angles // Face Away from Drivers - if (lookAway.getAsBoolean()) - { + if (lookAway.getAsBoolean()) { headingY = -1; } // Face Right - if (lookRight.getAsBoolean()) - { + if (lookRight.getAsBoolean()) { headingX = 1; } // Face Left - if (lookLeft.getAsBoolean()) - { + if (lookLeft.getAsBoolean()) { headingX = -1; } // Face Towards the Drivers - if (lookTowards.getAsBoolean()) - { + if (lookTowards.getAsBoolean()) { headingY = 1; } // Prevent Movement After Auto - if (resetHeading) - { - if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) - { + if (resetHeading) { + if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { // Get the curret Heading Rotation2d currentHeading = swerve.getHeading(); @@ -112,43 +107,46 @@ public void execute() headingX = currentHeading.getSin(); headingY = currentHeading.getCos(); } - //Dont reset Heading Again + // Dont reset Heading Again resetHeading = false; } - ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY); + ChassisSpeeds desiredSpeeds = + swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY); // Limit velocity to prevent tippy Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); - translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(), - Constants.LOOP_TIME, Constants.ROBOT_MASS, List.of(Constants.CHASSIS), - swerve.getSwerveDriveConfiguration()); + translation = + SwerveMath.limitVelocity( + translation, + swerve.getFieldVelocity(), + swerve.getPose(), + Constants.LOOP_TIME, + Constants.ROBOT_MASS, + List.of(Constants.CHASSIS), + swerve.getSwerveDriveConfiguration()); SmartDashboard.putNumber("LimitedTranslation", translation.getX()); SmartDashboard.putString("Translation", translation.toString()); // Make the robot move - if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) - { + if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { resetHeading = true; - swerve.drive(translation, (Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), true); - } else - { + swerve.drive( + translation, + (Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), + true); + } else { swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); } } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) - { - } + public void end(boolean interrupted) {} // Returns true when the command should end. @Override - public boolean isFinished() - { + public boolean isFinished() { return false; } - - -} \ No newline at end of file +} From 5bfcbfa5ab37f469294463a1b412be6a250fb978 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Wed, 6 Mar 2024 12:16:43 -0500 Subject: [PATCH 23/30] Improve dpad inputs --- src/main/java/frc/robot/RobotContainer.java | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3977b093..a31b5991 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -72,10 +72,12 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); - POVButton driverUp = new POVButton(driver, 0); - POVButton driverLeft = new POVButton(driver, 90); - POVButton driverDown = new POVButton(driver, 180); - POVButton driverRight = new POVButton(driver, 270); + int 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)); AbsoluteDrive closedAbsoluteDrive = new AbsoluteDrive( From 073a073e608a6eaa1d078ac9a59d9dd6f3083427 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Wed, 6 Mar 2024 12:20:24 -0500 Subject: [PATCH 24/30] Remove unused import --- src/main/java/frc/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a31b5991..2e2cc3e0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ 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.OperatorConstants; import frc.robot.commands.AbsoluteDrive; From f3854dc9e1c09342f19dc3a8ed989032a59951c5 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Wed, 6 Mar 2024 12:23:59 -0500 Subject: [PATCH 25/30] add rate limiter to intakes --- src/main/java/frc/robot/subsystems/Intake.java | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index be5df587..9b2d4878 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -6,6 +6,8 @@ import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; + +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -21,8 +23,10 @@ public class Intake extends SubsystemBase { private CANSparkMax motorRight = new CANSparkMax(Constants.Intake.rightCANID, CANSparkLowLevel.MotorType.kBrushless); + private SlewRateLimiter limiter = new SlewRateLimiter(0.5); + double speed = Constants.Intake.speed; - double currentSpeed = 0.0; + double targetSpeed = 0; public Intake() { motorLeft.setInverted(Constants.Intake.leftInverted); @@ -32,14 +36,11 @@ public Intake() { } private void set(double power) { - motorLeft.set(power); - motorCenter.set(power); - motorRight.set(power); - currentSpeed = power; + targetSpeed = power; } public void toggle() { - if (currentSpeed == 0.0) { + if (targetSpeed == 0.0) { start(); } else { stop(); @@ -66,6 +67,10 @@ public boolean isRunning() { public void periodic() { // This method will be called once per scheduler run SmartDashboard.putBoolean("Intake Running", isRunning()); + + motorLeft.set(limiter.calculate(targetSpeed)); + motorRight.set(limiter.calculate(targetSpeed)); + motorCenter.set(limiter.calculate(targetSpeed)); } public Command reverseIntakeCommand() { From 7b875135b56b1efa02c5f2bed367b182a007fae7 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Wed, 6 Mar 2024 12:25:57 -0500 Subject: [PATCH 26/30] whitespace fix --- src/main/java/frc/robot/subsystems/Intake.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 9b2d4878..90fd83a4 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -6,7 +6,6 @@ import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; - import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; From 42c9e0fa26ae5e54ace07e053e327264830807e5 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Thu, 7 Mar 2024 16:10:57 -0500 Subject: [PATCH 27/30] Swerve PID tuning --- .../deploy/swerves/KrakenSwerve/controllerproperties.json | 2 +- src/main/deploy/swerves/KrakenSwerve/modules/backright.json | 2 +- src/main/deploy/swerves/KrakenSwerve/modules/frontleft.json | 2 +- src/main/deploy/swerves/KrakenSwerve/modules/frontright.json | 2 +- .../swerves/KrakenSwerve/modules/physicalproperties.json | 2 +- .../deploy/swerves/KrakenSwerve/modules/pidfproperties.json | 4 ++-- 6 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/deploy/swerves/KrakenSwerve/controllerproperties.json b/src/main/deploy/swerves/KrakenSwerve/controllerproperties.json index 669097e7..9ef40425 100644 --- a/src/main/deploy/swerves/KrakenSwerve/controllerproperties.json +++ b/src/main/deploy/swerves/KrakenSwerve/controllerproperties.json @@ -2,7 +2,7 @@ "angleJoystickRadiusDeadband": 0.5, "heading": { "p": 0.4, - "i": 0, + "i": 0.1, "d": 0.01 } } \ No newline at end of file diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/backright.json b/src/main/deploy/swerves/KrakenSwerve/modules/backright.json index 7481ed76..09c7aeef 100644 --- a/src/main/deploy/swerves/KrakenSwerve/modules/backright.json +++ b/src/main/deploy/swerves/KrakenSwerve/modules/backright.json @@ -18,7 +18,7 @@ "drive": true, "angle": false }, - "absoluteEncoderOffset": 163, + "absoluteEncoderOffset": 164, "location": { "front": -13, "left": -13 diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/frontleft.json b/src/main/deploy/swerves/KrakenSwerve/modules/frontleft.json index fef30ebd..4a854b8b 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": 330, + "absoluteEncoderOffset": 135, "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 0db6e74b..542ac431 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": 18, + "absoluteEncoderOffset": 58, "location": { "front": 13, "left": -13 diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json b/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json index c192b5bf..ad1ea915 100644 --- a/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json +++ b/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json @@ -5,7 +5,7 @@ }, "currentLimit": { "drive": 70, - "angle": 60 + "angle": 40 }, "rampRate": { "drive": 0.05, diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/pidfproperties.json b/src/main/deploy/swerves/KrakenSwerve/modules/pidfproperties.json index e032133b..d3ab830e 100644 --- a/src/main/deploy/swerves/KrakenSwerve/modules/pidfproperties.json +++ b/src/main/deploy/swerves/KrakenSwerve/modules/pidfproperties.json @@ -1,13 +1,13 @@ { "drive": { "p": 1, - "i": 0.01, + "i": 0.1, "d": 0, "f": 0, "iz": 0 }, "angle": { - "p": 0.004, + "p": 0.1, "i": 0, "d": 1.5, "f": 0, From 5addbc97d76fafa2e2343b4734475d8fa20ef4e2 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Thu, 7 Mar 2024 16:11:13 -0500 Subject: [PATCH 28/30] Fix rate limiter addition error --- src/main/java/frc/robot/subsystems/Intake.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 90fd83a4..0e5e96ae 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -59,7 +59,7 @@ public void stop() { } public boolean isRunning() { - return !(motorLeft.get() == 0); + return !(targetSpeed == 0); } @Override From 12f662fc08e3b97fbc919b615c0f41e6b583f504 Mon Sep 17 00:00:00 2001 From: robo7660 Date: Thu, 7 Mar 2024 16:11:35 -0500 Subject: [PATCH 29/30] switch back to absolute drive --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2e2cc3e0..f3d8d28f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -100,7 +100,7 @@ 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)); From 3e5a784c5c7c0ca083bfa8f828069c9bc51e715f Mon Sep 17 00:00:00 2001 From: robo7660 Date: Thu, 7 Mar 2024 16:13:35 -0500 Subject: [PATCH 30/30] updated vendor deps --- vendordeps/PathplannerLib.json | 6 +++--- vendordeps/yagsl.json | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 787450f4..b849e926 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.4", + "version": "2024.2.5", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.4" + "version": "2024.2.5" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.4", + "version": "2024.2.5", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/yagsl.json b/vendordeps/yagsl.json index 8f7fdfda..c280eee6 100644 --- a/vendordeps/yagsl.json +++ b/vendordeps/yagsl.json @@ -1,7 +1,7 @@ { "fileName": "yagsl.json", "name": "YAGSL", - "version": "2024.4.8.5", + "version": "2024.4.8.6", "frcYear": "2024", "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "swervelib", "artifactId": "YAGSL-java", - "version": "2024.4.8.5" + "version": "2024.4.8.6" } ], "jniDependencies": [],