From cdd19dcac294aef52662b291a7126c7b1d478d26 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Fri, 1 Mar 2024 19:11:04 -0500 Subject: [PATCH 01/28] fix(arm): Set initial arm state. --- src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/arm/Arm.java | 5 +---- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5cc4c9e..217e6f8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -60,7 +60,6 @@ public static RobotContainer getInstance() { private void initializeTelemetry() { if (RobotConstants.USE_TELEMETRY) { Telemetry.initializeShuffleboards(arm); - SmartDashboard.putData("Arm Mechanism", RobotMechanisms.getInstance().getMechanism()); } SmartDashboard.putData(auto.getAutonomousChooser()); diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index d297d80..b4aa208 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.lib.Subsystem; import frc.lib.Telemetry; -import frc.robot.RobotMechanisms; import frc.robot.arm.LimitSwitchIO.LimitSwitchIOValues; import frc.robot.arm.ShoulderMotorIO.ShoulderMotorIOValues; import frc.robot.arm.WristMotorIO.WristMotorIOValues; @@ -48,7 +47,7 @@ private Arm() { shoulderMotor.configure(); wristMotor.configure(); - ArmState initialState = ArmState.STOW.withShoulder(Rotation2d.fromDegrees(45)); + ArmState initialState = ArmState.INIT; setPosition(initialState); @@ -82,8 +81,6 @@ public void periodic() { } setSetpoint(setpoint.nextSetpoint(goal)); - - RobotMechanisms.getInstance().setArmState(getPosition()); } @Override From 9f0f5f2797506d6d0ea14760980ee61f9bc1b9c3 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Fri, 1 Mar 2024 19:54:03 -0500 Subject: [PATCH 02/28] feat(climber): Add climber. --- src/main/java/frc/robot/RobotContainer.java | 8 ++- src/main/java/frc/robot/climber/Climber.java | 62 ++++++++++++++++--- .../frc/robot/climber/ClimberFactory.java | 14 +++-- .../java/frc/robot/climber/ElevatorIO.java | 6 +- .../java/frc/robot/climber/ElevatorIOSim.java | 20 +----- .../frc/robot/climber/ElevatorIOSparkMax.java | 57 +++++++++++++++++ 6 files changed, 133 insertions(+), 34 deletions(-) create mode 100644 src/main/java/frc/robot/climber/ElevatorIOSparkMax.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 217e6f8..56ad8ac 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,7 @@ import frc.lib.Telemetry; import frc.robot.arm.Arm; import frc.robot.auto.Auto; +import frc.robot.climber.Climber; import frc.robot.intake.Intake; import frc.robot.odometry.Odometry; import frc.robot.shooter.Shooter; @@ -20,6 +21,7 @@ public class RobotContainer { private final Arm arm; private final Auto auto; + private final Climber climber; private final Intake intake; private final Odometry odometry; private final Shooter shooter; @@ -31,6 +33,7 @@ public class RobotContainer { private RobotContainer() { arm = Arm.getInstance(); auto = Auto.getInstance(); + climber = Climber.getInstance(); intake = Intake.getInstance(); odometry = Odometry.getInstance(); shooter = Shooter.getInstance(); @@ -59,7 +62,7 @@ public static RobotContainer getInstance() { /** Initializes subsystem telemetry. */ private void initializeTelemetry() { if (RobotConstants.USE_TELEMETRY) { - Telemetry.initializeShuffleboards(arm); + Telemetry.initializeShuffleboards(arm, climber); } SmartDashboard.putData(auto.getAutonomousChooser()); @@ -89,6 +92,9 @@ private void configureBindings() { .whileFalse(auto.stow()); operatorController.rightBumper().whileTrue(shooter.serialize()); + operatorController.povUp().whileTrue(climber.up()); + operatorController.povDown().whileTrue(climber.down()); + operatorController.a().whileTrue(arm.amp()).whileFalse(auto.stow()); } diff --git a/src/main/java/frc/robot/climber/Climber.java b/src/main/java/frc/robot/climber/Climber.java index caa901c..fa3ebc5 100644 --- a/src/main/java/frc/robot/climber/Climber.java +++ b/src/main/java/frc/robot/climber/Climber.java @@ -2,6 +2,9 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.lib.CAN; import frc.lib.Subsystem; import frc.lib.Telemetry; import frc.robot.climber.ClimberConstants.ElevatorConstants; @@ -14,16 +17,27 @@ public class Climber extends Subsystem { private static Climber instance = null; /** Elevator. */ - private final ElevatorIO elevator; + private final ElevatorIO eastElevator; /** Elevator values. */ - private final ElevatorIOValues elevatorValues = new ElevatorIOValues(); + private final ElevatorIOValues eastElevatorValues = new ElevatorIOValues(); + + /** Elevator. */ + private final ElevatorIO westElevator; + + /** Elevator values. */ + private final ElevatorIOValues westElevatorValues = new ElevatorIOValues(); /** Creates a new instance of the climber subsystem. */ private Climber() { - elevator = ClimberFactory.createElevator(); + westElevator = ClimberFactory.createElevator(new CAN(6), false); + eastElevator = ClimberFactory.createElevator(new CAN(7), false); - elevator.setPosition(ElevatorConstants.MIN_HEIGHT); + westElevator.configure(); + eastElevator.configure(); + + westElevator.setPosition(ElevatorConstants.MIN_HEIGHT); + eastElevator.setPosition(ElevatorConstants.MIN_HEIGHT); } /** @@ -40,12 +54,46 @@ public static Climber getInstance() { } @Override - public void periodic() {} + public void periodic() { + westElevator.update(westElevatorValues); + eastElevator.update(eastElevatorValues); + } @Override public void addToShuffleboard(ShuffleboardTab tab) { - ShuffleboardLayout elevator = Telemetry.addColumn(tab, "Elevator"); + ShuffleboardLayout west = Telemetry.addColumn(tab, "West"); + + west.addDouble("Position (m)", () -> westElevatorValues.positionMeters); + + ShuffleboardLayout east = Telemetry.addColumn(tab, "East"); + + east.addDouble("Position (m)", () -> eastElevatorValues.positionMeters); + } + + public void stop() { + westElevator.setVoltage(0); + eastElevator.setVoltage(0); + } + + public Command up() { + double upVoltage = 1.0; + + return Commands.parallel( + Commands.run(() -> westElevator.setVoltage(upVoltage)) + .until(() -> westElevatorValues.positionMeters >= ElevatorConstants.MAX_HEIGHT), + Commands.run(() -> eastElevator.setVoltage(upVoltage)) + .until(() -> eastElevatorValues.positionMeters >= ElevatorConstants.MAX_HEIGHT)) + .finallyDo(this::stop); + } + + public Command down() { + double downVoltage = -1.0; - elevator.addDouble("Position (m)", () -> elevatorValues.positionMeters); + return Commands.parallel( + Commands.run(() -> westElevator.setVoltage(downVoltage)) + .until(() -> westElevatorValues.positionMeters <= ElevatorConstants.MIN_HEIGHT), + Commands.run(() -> eastElevator.setVoltage(downVoltage)) + .until(() -> eastElevatorValues.positionMeters <= ElevatorConstants.MIN_HEIGHT)) + .finallyDo(this::stop); } } diff --git a/src/main/java/frc/robot/climber/ClimberFactory.java b/src/main/java/frc/robot/climber/ClimberFactory.java index ef46b10..a78dc4b 100644 --- a/src/main/java/frc/robot/climber/ClimberFactory.java +++ b/src/main/java/frc/robot/climber/ClimberFactory.java @@ -1,5 +1,10 @@ package frc.robot.climber; +import frc.lib.CAN; +import frc.robot.Robot; +import frc.robot.RobotConstants; +import frc.robot.RobotConstants.Subsystem; + /** Helper class for creating hardware for the climber subsystem. */ public class ClimberFactory { @@ -8,11 +13,10 @@ public class ClimberFactory { * * @return an elevator. */ - public static ElevatorIO createElevator() { - // TODO - // if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.CLIMBER)) { - // return new ElevatorIONeo(); - // } + public static ElevatorIO createElevator(CAN can, boolean inverted) { + if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.CLIMBER)) { + return new ElevatorIOSparkMax(can, inverted); + } return new ElevatorIOSim(); } diff --git a/src/main/java/frc/robot/climber/ElevatorIO.java b/src/main/java/frc/robot/climber/ElevatorIO.java index 82c5e88..c7d1967 100644 --- a/src/main/java/frc/robot/climber/ElevatorIO.java +++ b/src/main/java/frc/robot/climber/ElevatorIO.java @@ -27,9 +27,9 @@ public static class ElevatorIOValues { public void setPosition(double positionMeters); /** - * Sets the elevator's setpoint. + * Sets the elevator's voltage. * - * @param positionMeters the elevator's setpoint. + * @param volts the elevator's voltage. */ - public void setSetpoint(double positionMeters); + public void setVoltage(double volts); } diff --git a/src/main/java/frc/robot/climber/ElevatorIOSim.java b/src/main/java/frc/robot/climber/ElevatorIOSim.java index 5cd4fb3..0687369 100644 --- a/src/main/java/frc/robot/climber/ElevatorIOSim.java +++ b/src/main/java/frc/robot/climber/ElevatorIOSim.java @@ -1,7 +1,5 @@ package frc.robot.climber; -import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.ElevatorSim; import frc.robot.RobotConstants; @@ -14,10 +12,6 @@ public class ElevatorIOSim implements ElevatorIO { private final ElevatorSim elevatorSim; - private final PIDController feedback; - - private final ElevatorFeedforward feedforward; - public ElevatorIOSim() { motor = DCMotor.getNEO(1); @@ -31,12 +25,6 @@ public ElevatorIOSim() { ElevatorConstants.MAX_HEIGHT, true, ElevatorConstants.MIN_HEIGHT); - - // TODO - feedback = new PIDController(1.0, 0.0, 0.0); - - // TODO - feedforward = new ElevatorFeedforward(0, 0, 0); } @Override @@ -55,11 +43,7 @@ public void setPosition(double positionMeters) { } @Override - public void setSetpoint(double positionMeters) { - double feedbackVolts = feedback.calculate(elevatorSim.getPositionMeters(), positionMeters); - - double feedforwardVolts = feedforward.calculate(elevatorSim.getVelocityMetersPerSecond()); - - elevatorSim.setInputVoltage(feedbackVolts + feedforwardVolts); + public void setVoltage(double volts) { + elevatorSim.setInputVoltage(volts); } } diff --git a/src/main/java/frc/robot/climber/ElevatorIOSparkMax.java b/src/main/java/frc/robot/climber/ElevatorIOSparkMax.java new file mode 100644 index 0000000..2b32da3 --- /dev/null +++ b/src/main/java/frc/robot/climber/ElevatorIOSparkMax.java @@ -0,0 +1,57 @@ +package frc.robot.climber; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import frc.lib.CAN; +import frc.lib.Configurator; + +public class ElevatorIOSparkMax implements ElevatorIO { + + private final CANSparkMax sparkMax; + + private final boolean isInverted; + + private static final double GEARING = 25.0; + + private static final double METERS_PER_ROTATION = 1.0; + + public ElevatorIOSparkMax(CAN can, boolean invert) { + sparkMax = new CANSparkMax(can.id(), MotorType.kBrushless); + + isInverted = invert; + } + + @Override + public void configure() { + sparkMax.setInverted(isInverted); + + Configurator.configureREV(() -> sparkMax.setIdleMode(IdleMode.kBrake)); + } + + @Override + public void update(ElevatorIOValues values) { + values.positionMeters = getPositionRotations() * METERS_PER_ROTATION; + } + + /** + * Gets the absolute position in meters. + * + * @return the absolute position in meters. + */ + private double getPositionRotations() { + return sparkMax.getEncoder().getPosition() / GEARING; + } + + @Override + public void setPosition(double positionMeters) { + double rotations = positionMeters / METERS_PER_ROTATION; + + sparkMax.getEncoder().setPosition(rotations); + } + + @Override + public void setVoltage(double volts) { + sparkMax.setVoltage(volts); + } +} From d30e8f2865e145df574cf47a814d96370bd52151 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Fri, 1 Mar 2024 19:57:12 -0500 Subject: [PATCH 03/28] refactor(climber): Use correct gearing. --- src/main/java/frc/robot/climber/ClimberConstants.java | 2 +- src/main/java/frc/robot/climber/ElevatorIOSparkMax.java | 9 ++++----- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/climber/ClimberConstants.java b/src/main/java/frc/robot/climber/ClimberConstants.java index aea37f2..87ebe6b 100644 --- a/src/main/java/frc/robot/climber/ClimberConstants.java +++ b/src/main/java/frc/robot/climber/ClimberConstants.java @@ -8,7 +8,7 @@ public class ClimberConstants { /** Constants for the elevator. */ public static class ElevatorConstants { /** Gearing between the elevator motor and drum. */ - public static final double GEARING = 1.0; + public static final double GEARING = 25.0; /** Mass of the elevator carriage in kilograms. */ // TODO Does not include the mass of two WCP-0418 parts diff --git a/src/main/java/frc/robot/climber/ElevatorIOSparkMax.java b/src/main/java/frc/robot/climber/ElevatorIOSparkMax.java index 2b32da3..937dfb0 100644 --- a/src/main/java/frc/robot/climber/ElevatorIOSparkMax.java +++ b/src/main/java/frc/robot/climber/ElevatorIOSparkMax.java @@ -5,6 +5,7 @@ import com.revrobotics.CANSparkMax; import frc.lib.CAN; import frc.lib.Configurator; +import frc.robot.climber.ClimberConstants.ElevatorConstants; public class ElevatorIOSparkMax implements ElevatorIO { @@ -12,8 +13,6 @@ public class ElevatorIOSparkMax implements ElevatorIO { private final boolean isInverted; - private static final double GEARING = 25.0; - private static final double METERS_PER_ROTATION = 1.0; public ElevatorIOSparkMax(CAN can, boolean invert) { @@ -35,12 +34,12 @@ public void update(ElevatorIOValues values) { } /** - * Gets the absolute position in meters. + * Gets the absolute position in rotations. * - * @return the absolute position in meters. + * @return the absolute position in rotations. */ private double getPositionRotations() { - return sparkMax.getEncoder().getPosition() / GEARING; + return sparkMax.getEncoder().getPosition() / ElevatorConstants.GEARING; } @Override From e9818c899f7358e292017144d93dae8efae0e08e Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Fri, 1 Mar 2024 20:03:30 -0500 Subject: [PATCH 04/28] feat(climber): Add climb commands. --- simgui.json | 35 +++++-------------- src/main/java/frc/robot/climber/Climber.java | 32 +++++++++++------ .../java/frc/robot/climber/ElevatorIOSim.java | 2 +- 3 files changed, 31 insertions(+), 38 deletions(-) diff --git a/simgui.json b/simgui.json index bc02272..7742a03 100644 --- a/simgui.json +++ b/simgui.json @@ -23,16 +23,6 @@ "window": { "visible": true } - }, - "/SmartDashboard/Arm Mechanism": { - "window": { - "visible": true - } - }, - "/SmartDashboard/VisionSystemSim-visionSystem/Sim Field": { - "window": { - "visible": true - } } } }, @@ -43,37 +33,28 @@ "transitory": { "Shuffleboard": { "Arm": { - "Derivatives": { - "open": true - }, "Feedforward": { "open": true }, - "Goal": { + "Velocities": { "open": true - }, - "Position": { + } + }, + "Climber": { + "East": { "open": true }, - "Setpoint": { + "Elevator": { "open": true }, - "Velocities": { + "Linear Actuator": { "open": true }, - "Voltages": { + "West": { "open": true }, "open": true }, - "Climber": { - "Elevator": { - "open": true - }, - "Linear Actuator": { - "open": true - } - }, "Intake": { "Roller": { "open": true diff --git a/src/main/java/frc/robot/climber/Climber.java b/src/main/java/frc/robot/climber/Climber.java index fa3ebc5..79b1502 100644 --- a/src/main/java/frc/robot/climber/Climber.java +++ b/src/main/java/frc/robot/climber/Climber.java @@ -70,30 +70,42 @@ public void addToShuffleboard(ShuffleboardTab tab) { east.addDouble("Position (m)", () -> eastElevatorValues.positionMeters); } + public boolean westAtTop() { + return westElevatorValues.positionMeters >= ElevatorConstants.MAX_HEIGHT; + } + + public boolean westAtBottom() { + return westElevatorValues.positionMeters <= ElevatorConstants.MIN_HEIGHT; + } + + public boolean eastAtTop() { + return eastElevatorValues.positionMeters >= ElevatorConstants.MAX_HEIGHT; + } + + public boolean eastAtBottom() { + return eastElevatorValues.positionMeters <= ElevatorConstants.MIN_HEIGHT; + } + public void stop() { westElevator.setVoltage(0); eastElevator.setVoltage(0); } public Command up() { - double upVoltage = 1.0; + double upVoltage = 4.0; return Commands.parallel( - Commands.run(() -> westElevator.setVoltage(upVoltage)) - .until(() -> westElevatorValues.positionMeters >= ElevatorConstants.MAX_HEIGHT), - Commands.run(() -> eastElevator.setVoltage(upVoltage)) - .until(() -> eastElevatorValues.positionMeters >= ElevatorConstants.MAX_HEIGHT)) + Commands.run(() -> westElevator.setVoltage(upVoltage)).until(this::westAtTop), + Commands.run(() -> eastElevator.setVoltage(upVoltage)).until(this::eastAtTop)) .finallyDo(this::stop); } public Command down() { - double downVoltage = -1.0; + double downVoltage = -4.0; return Commands.parallel( - Commands.run(() -> westElevator.setVoltage(downVoltage)) - .until(() -> westElevatorValues.positionMeters <= ElevatorConstants.MIN_HEIGHT), - Commands.run(() -> eastElevator.setVoltage(downVoltage)) - .until(() -> eastElevatorValues.positionMeters <= ElevatorConstants.MIN_HEIGHT)) + Commands.run(() -> westElevator.setVoltage(downVoltage)).until(this::westAtBottom), + Commands.run(() -> eastElevator.setVoltage(downVoltage)).until(this::eastAtBottom)) .finallyDo(this::stop); } } diff --git a/src/main/java/frc/robot/climber/ElevatorIOSim.java b/src/main/java/frc/robot/climber/ElevatorIOSim.java index 0687369..2c50f4a 100644 --- a/src/main/java/frc/robot/climber/ElevatorIOSim.java +++ b/src/main/java/frc/robot/climber/ElevatorIOSim.java @@ -23,7 +23,7 @@ public ElevatorIOSim() { ElevatorConstants.DRUM_DIAMETER, ElevatorConstants.MIN_HEIGHT, ElevatorConstants.MAX_HEIGHT, - true, + false, ElevatorConstants.MIN_HEIGHT); } From b6718c07a9bdd064f1d005b196d245652ca4b09c Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Fri, 1 Mar 2024 20:08:00 -0500 Subject: [PATCH 05/28] refactor(climber): Move voltage. --- src/main/java/frc/robot/climber/Climber.java | 16 ++++++++-------- .../java/frc/robot/climber/ClimberConstants.java | 6 ++++++ 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/climber/Climber.java b/src/main/java/frc/robot/climber/Climber.java index 79b1502..08a3700 100644 --- a/src/main/java/frc/robot/climber/Climber.java +++ b/src/main/java/frc/robot/climber/Climber.java @@ -92,20 +92,20 @@ public void stop() { } public Command up() { - double upVoltage = 4.0; - return Commands.parallel( - Commands.run(() -> westElevator.setVoltage(upVoltage)).until(this::westAtTop), - Commands.run(() -> eastElevator.setVoltage(upVoltage)).until(this::eastAtTop)) + Commands.run(() -> westElevator.setVoltage(ElevatorConstants.UP_VOLTAGE)) + .until(this::westAtTop), + Commands.run(() -> eastElevator.setVoltage(ElevatorConstants.UP_VOLTAGE)) + .until(this::eastAtTop)) .finallyDo(this::stop); } public Command down() { - double downVoltage = -4.0; - return Commands.parallel( - Commands.run(() -> westElevator.setVoltage(downVoltage)).until(this::westAtBottom), - Commands.run(() -> eastElevator.setVoltage(downVoltage)).until(this::eastAtBottom)) + Commands.run(() -> westElevator.setVoltage(ElevatorConstants.DOWN_VOLTAGE)) + .until(this::westAtBottom), + Commands.run(() -> eastElevator.setVoltage(ElevatorConstants.DOWN_VOLTAGE)) + .until(this::eastAtBottom)) .finallyDo(this::stop); } } diff --git a/src/main/java/frc/robot/climber/ClimberConstants.java b/src/main/java/frc/robot/climber/ClimberConstants.java index 87ebe6b..cd14e99 100644 --- a/src/main/java/frc/robot/climber/ClimberConstants.java +++ b/src/main/java/frc/robot/climber/ClimberConstants.java @@ -23,5 +23,11 @@ public static class ElevatorConstants { /** Maximum height of the elevator in meters. */ public static final double MAX_HEIGHT = Units.inchesToMeters(28); + + /** Voltage to be applied while going up. */ + public static final double UP_VOLTAGE = 1; + + /** Voltage to be applied wihle going down. */ + public static final double DOWN_VOLTAGE = -1; } } From 0504a50b38816a755a1a9922a32017fa99e4cf92 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Fri, 1 Mar 2024 21:40:13 -0500 Subject: [PATCH 06/28] fix(climber): Configure Spark Maxes. --- src/main/java/frc/robot/RobotConstants.java | 6 +++--- src/main/java/frc/robot/climber/ClimberConstants.java | 4 ++-- src/main/java/frc/robot/climber/ElevatorIOSparkMax.java | 2 ++ 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotConstants.java b/src/main/java/frc/robot/RobotConstants.java index f217aeb..6e68847 100644 --- a/src/main/java/frc/robot/RobotConstants.java +++ b/src/main/java/frc/robot/RobotConstants.java @@ -44,7 +44,7 @@ public enum Subsystem { SWERVE, } - public static final Set REAL_SUBSYSTEMS = - EnumSet.of( - Subsystem.ARM, Subsystem.INTAKE, Subsystem.ODOMETRY, Subsystem.SHOOTER, Subsystem.SWERVE); + public static final Set ALL_SUBSYSTEMS = EnumSet.of(Subsystem.ARM, Subsystem.CLIMBER, Subsystem.INTAKE, Subsystem.ODOMETRY, Subsystem.SHOOTER, Subsystem.SWERVE); + + public static final Set REAL_SUBSYSTEMS = ALL_SUBSYSTEMS; } diff --git a/src/main/java/frc/robot/climber/ClimberConstants.java b/src/main/java/frc/robot/climber/ClimberConstants.java index cd14e99..afdc2ab 100644 --- a/src/main/java/frc/robot/climber/ClimberConstants.java +++ b/src/main/java/frc/robot/climber/ClimberConstants.java @@ -25,9 +25,9 @@ public static class ElevatorConstants { public static final double MAX_HEIGHT = Units.inchesToMeters(28); /** Voltage to be applied while going up. */ - public static final double UP_VOLTAGE = 1; + public static final double UP_VOLTAGE = 2; /** Voltage to be applied wihle going down. */ - public static final double DOWN_VOLTAGE = -1; + public static final double DOWN_VOLTAGE = 1; } } diff --git a/src/main/java/frc/robot/climber/ElevatorIOSparkMax.java b/src/main/java/frc/robot/climber/ElevatorIOSparkMax.java index 937dfb0..0c205d4 100644 --- a/src/main/java/frc/robot/climber/ElevatorIOSparkMax.java +++ b/src/main/java/frc/robot/climber/ElevatorIOSparkMax.java @@ -26,6 +26,8 @@ public void configure() { sparkMax.setInverted(isInverted); Configurator.configureREV(() -> sparkMax.setIdleMode(IdleMode.kBrake)); + + Configurator.configureStatusFrames(sparkMax); } @Override From 2bf659fdc9613b5005d0061486de3cc04e408dbb Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 08:08:51 -0500 Subject: [PATCH 07/28] feat(climber): While climbing, go into intake position. --- src/main/java/frc/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 56ad8ac..9b17981 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.lib.Telemetry; import frc.robot.arm.Arm; @@ -93,7 +94,7 @@ private void configureBindings() { operatorController.rightBumper().whileTrue(shooter.serialize()); operatorController.povUp().whileTrue(climber.up()); - operatorController.povDown().whileTrue(climber.down()); + operatorController.povDown().whileTrue(Commands.parallel(auto.readyIntake().repeatedly(), climber.down())); operatorController.a().whileTrue(arm.amp()).whileFalse(auto.stow()); } From c13adba961e8be9e94c79377a2490c4ca6c2b48a Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 08:55:36 -0500 Subject: [PATCH 08/28] feat(auto): Add autos. --- src/main/deploy/pathplanner/autos/amp.auto | 49 +++++++++- src/main/deploy/pathplanner/autos/center.auto | 61 ++++++++++++ src/main/deploy/pathplanner/autos/stage.auto | 74 +++++++++++++++ .../pathplanner/paths/amp.toNearest.path | 17 ++++ .../pathplanner/paths/center.toNearest.path | 70 ++++++++++++++ .../pathplanner/paths/center.toSubwoofer.path | 52 +++++++++++ .../pathplanner/paths/stage.toNearest.path | 93 +++++++++++++++++++ 7 files changed, 413 insertions(+), 3 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/center.auto create mode 100644 src/main/deploy/pathplanner/autos/stage.auto create mode 100644 src/main/deploy/pathplanner/paths/center.toNearest.path create mode 100644 src/main/deploy/pathplanner/paths/center.toSubwoofer.path create mode 100644 src/main/deploy/pathplanner/paths/stage.toNearest.path diff --git a/src/main/deploy/pathplanner/autos/amp.auto b/src/main/deploy/pathplanner/autos/amp.auto index 5695d2b..9efcd6b 100644 --- a/src/main/deploy/pathplanner/autos/amp.auto +++ b/src/main/deploy/pathplanner/autos/amp.auto @@ -2,26 +2,69 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, - "y": 6.63 + "x": 1.36, + "y": 5.56 }, - "rotation": 60.0 + "rotation": 0.0 }, "command": { "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "home" + } + }, + { + "type": "named", + "data": { + "name": "readyShoot" + } + }, + { + "type": "named", + "data": { + "name": "shootNote" + } + }, { "type": "path", "data": { "pathName": "amp.toNearest" } }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeNote" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, { "type": "path", "data": { "pathName": "amp.toSubwoofer" } + }, + { + "type": "named", + "data": { + "name": "shootNote" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/center.auto b/src/main/deploy/pathplanner/autos/center.auto new file mode 100644 index 0000000..836de3c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/center.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.36, + "y": 5.56 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "home" + } + }, + { + "type": "named", + "data": { + "name": "readyShoot" + } + }, + { + "type": "named", + "data": { + "name": "shootNote" + } + }, + { + "type": "path", + "data": { + "pathName": "center.toNearest" + } + }, + { + "type": "named", + "data": { + "name": "intakeNote" + } + }, + { + "type": "path", + "data": { + "pathName": "center.toSubwoofer" + } + }, + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/stage.auto b/src/main/deploy/pathplanner/autos/stage.auto new file mode 100644 index 0000000..c2fb194 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/stage.auto @@ -0,0 +1,74 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 4.39 + }, + "rotation": -60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "home" + } + }, + { + "type": "named", + "data": { + "name": "readyShoot" + } + }, + { + "type": "named", + "data": { + "name": "shootNote" + } + }, + { + "type": "path", + "data": { + "pathName": "stage.toNearest" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeNote" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "stage.toNearest" + } + }, + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/amp.toNearest.path b/src/main/deploy/pathplanner/paths/amp.toNearest.path index 3ef3105..c29a804 100644 --- a/src/main/deploy/pathplanner/paths/amp.toNearest.path +++ b/src/main/deploy/pathplanner/paths/amp.toNearest.path @@ -58,6 +58,23 @@ ] } } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 0.7, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeNote" + } + } + ] + } + } } ], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/center.toNearest.path b/src/main/deploy/pathplanner/paths/center.toNearest.path new file mode 100644 index 0000000..ccad971 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/center.toNearest.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3633965729469937, + "y": 5.555088802485298 + }, + "prevControl": null, + "nextControl": { + "x": 2.3633965729469937, + "y": 5.555088802485298 + }, + "isLocked": false, + "linkedName": "center.subwoofer" + }, + { + "anchor": { + "x": 2.409292510892716, + "y": 5.555088802485298 + }, + "prevControl": { + "x": 1.409292510892716, + "y": 5.555088802485298 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "center.nearestNote" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.8, + "command": { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeNote" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/center.toSubwoofer.path b/src/main/deploy/pathplanner/paths/center.toSubwoofer.path new file mode 100644 index 0000000..6a9d2c6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/center.toSubwoofer.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.409292510892716, + "y": 5.555088802485298 + }, + "prevControl": null, + "nextControl": { + "x": 3.409292510892716, + "y": 5.555088802485298 + }, + "isLocked": false, + "linkedName": "center.nearestNote" + }, + { + "anchor": { + "x": 1.3633965729469937, + "y": 5.555088802485298 + }, + "prevControl": { + "x": 1.7284734569469153, + "y": 5.584689630917723 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "center.subwoofer" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/stage.toNearest.path b/src/main/deploy/pathplanner/paths/stage.toNearest.path new file mode 100644 index 0000000..a1fee34 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/stage.toNearest.path @@ -0,0 +1,93 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7319122330552369, + "y": 4.390789550809871 + }, + "prevControl": null, + "nextControl": { + "x": 0.8503155467849413, + "y": 4.272386237080167 + }, + "isLocked": false, + "linkedName": "stage.subwoofer" + }, + { + "anchor": { + "x": 2.290889197163011, + "y": 4.124382094918037 + }, + "prevControl": { + "x": 1.2908891971630112, + "y": 4.124382094918037 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "stage.nearestNote" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.1, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] + } + } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 0.6, + "command": { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeNote" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -59.88626684901758, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file From 3ab4b8ab1840366eb8c0496d3efa1cdbd27b23aa Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 08:55:54 -0500 Subject: [PATCH 09/28] fix(arm): Spin slower. --- src/main/java/frc/robot/Robot.java | 3 +-- src/main/java/frc/robot/arm/ArmConstants.java | 2 +- src/main/java/frc/robot/auto/Auto.java | 3 ++- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 001a630..dee8f36 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.arm.Arm; import frc.robot.swerve.Swerve; public class Robot extends TimedRobot { @@ -19,7 +18,7 @@ public void robotInit() { robotContainer = RobotContainer.getInstance(); swerve = Swerve.getInstance(); - new Trigger(this::isEnabled).whileTrue(Arm.getInstance().home()); + // new Trigger(this::isEnabled).whileTrue(Arm.getInstance().home()); new Trigger(this::isDisabled) .debounce(RobotConstants.DISABLE_COAST_DELAY) diff --git a/src/main/java/frc/robot/arm/ArmConstants.java b/src/main/java/frc/robot/arm/ArmConstants.java index 898cc7d..98ea935 100644 --- a/src/main/java/frc/robot/arm/ArmConstants.java +++ b/src/main/java/frc/robot/arm/ArmConstants.java @@ -91,7 +91,7 @@ public static class WristMotorConstants { public static final double KP = 48.0; /** Maximum speed of the wrist joint in rotations per second. */ - public static final double MAXIMUM_SPEED = 2.4; + public static final double MAXIMUM_SPEED = 1.2; /** Maximum acceleration of the wrist joint in rotations per second per second. */ public static final double MAXIMUM_ACCELERATION = diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index e15b79d..7685693 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -82,6 +82,7 @@ private Auto() { AllianceFlipHelper::shouldFlip, swerve); + NamedCommands.registerCommand("home", Arm.getInstance().home()); NamedCommands.registerCommand("readyIntake", readyIntake()); NamedCommands.registerCommand("intakeNote", intakeNote()); NamedCommands.registerCommand("readyShoot", readyShoot()); @@ -136,7 +137,7 @@ public Command readyIntake() { } public Command intakeNote() { - return Commands.parallel(intake.intake(), shooter.intake()); + return readyIntake().andThen(Commands.parallel(intake.intake(), shooter.intake())); } public Command stow() { From 5474cbf46fae7370fb0fa203b8c5eb9cf392a3c7 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 09:01:37 -0500 Subject: [PATCH 10/28] feat(auto): Update some more paths. --- src/main/deploy/pathplanner/autos/amp.auto | 6 +- src/main/deploy/pathplanner/autos/stage.auto | 2 +- .../pathplanner/paths/stage.toNearest.path | 8 +-- .../pathplanner/paths/stage.toSubwoofer.path | 70 +++++++++++++++++++ 4 files changed, 78 insertions(+), 8 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/stage.toSubwoofer.path diff --git a/src/main/deploy/pathplanner/autos/amp.auto b/src/main/deploy/pathplanner/autos/amp.auto index 9efcd6b..c456e87 100644 --- a/src/main/deploy/pathplanner/autos/amp.auto +++ b/src/main/deploy/pathplanner/autos/amp.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.36, - "y": 5.56 + "x": 0.8, + "y": 6.63 }, - "rotation": 0.0 + "rotation": 60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/stage.auto b/src/main/deploy/pathplanner/autos/stage.auto index c2fb194..57476a3 100644 --- a/src/main/deploy/pathplanner/autos/stage.auto +++ b/src/main/deploy/pathplanner/autos/stage.auto @@ -57,7 +57,7 @@ { "type": "path", "data": { - "pathName": "stage.toNearest" + "pathName": "stage.toSubwoofer" } }, { diff --git a/src/main/deploy/pathplanner/paths/stage.toNearest.path b/src/main/deploy/pathplanner/paths/stage.toNearest.path index a1fee34..b7933bb 100644 --- a/src/main/deploy/pathplanner/paths/stage.toNearest.path +++ b/src/main/deploy/pathplanner/paths/stage.toNearest.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.7319122330552369, - "y": 4.390789550809871 + "x": 0.7636103146671326, + "y": 4.403274541213847 }, "prevControl": null, "nextControl": { - "x": 0.8503155467849413, - "y": 4.272386237080167 + "x": 0.8820136283968368, + "y": 4.2848712274841425 }, "isLocked": false, "linkedName": "stage.subwoofer" diff --git a/src/main/deploy/pathplanner/paths/stage.toSubwoofer.path b/src/main/deploy/pathplanner/paths/stage.toSubwoofer.path new file mode 100644 index 0000000..bd29c30 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/stage.toSubwoofer.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.290889197163011, + "y": 4.124382094918037 + }, + "prevControl": null, + "nextControl": { + "x": 1.7356097479669974, + "y": 3.8879977331994615 + }, + "isLocked": false, + "linkedName": "stage.nearestNote" + }, + { + "anchor": { + "x": 0.7636103146671326, + "y": 4.403274541213847 + }, + "prevControl": { + "x": 0.7636103146671326, + "y": 4.028527771748839 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "stage.subwoofer" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.1, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "readyShoot" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -53.123077204898124, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -1.0045856204165913, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file From 40e0763d2a4691dd65659faa568c0e74b5780513 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 09:07:22 -0500 Subject: [PATCH 11/28] fix(swerve): Flip drive direction. --- src/main/java/frc/robot/swerve/DriveRequest.java | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/main/java/frc/robot/swerve/DriveRequest.java b/src/main/java/frc/robot/swerve/DriveRequest.java index b9e1230..fff747e 100644 --- a/src/main/java/frc/robot/swerve/DriveRequest.java +++ b/src/main/java/frc/robot/swerve/DriveRequest.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.lib.AllianceFlipHelper; public record DriveRequest( DriveRequest.TranslationMode translationMode, @@ -39,11 +40,20 @@ public static DriveRequest fromController(CommandXboxController controller) { boolean aligningRequested = controller.rightTrigger().getAsBoolean(); double translationX = -controller.getLeftY(); + + if (AllianceFlipHelper.shouldFlip()) { + translationX *= -1; + } + double translationY = -controller.getLeftX(); double translationMagnitude = Math.hypot(translationX, translationY); Rotation2d translationDirection = new Rotation2d(translationX, translationY); + if (AllianceFlipHelper.shouldFlip()) { + translationDirection.plus(Rotation2d.fromDegrees(180)); + } + translationMagnitude = MathUtil.applyDeadband(translationMagnitude, 0.1); translationMagnitude = Math.copySign(translationMagnitude * translationMagnitude, translationMagnitude); From e16f30a50769046718121331926a570b022b93ec Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 09:31:14 -0500 Subject: [PATCH 12/28] feat(auto): Add auto for homing. --- src/main/deploy/pathplanner/autos/home.auto | 25 +++++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 2 +- 2 files changed, 26 insertions(+), 1 deletion(-) create mode 100644 src/main/deploy/pathplanner/autos/home.auto diff --git a/src/main/deploy/pathplanner/autos/home.auto b/src/main/deploy/pathplanner/autos/home.auto new file mode 100644 index 0000000..e8893f0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/home.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 2 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "home" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9b17981..6f600d7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -63,7 +63,7 @@ public static RobotContainer getInstance() { /** Initializes subsystem telemetry. */ private void initializeTelemetry() { if (RobotConstants.USE_TELEMETRY) { - Telemetry.initializeShuffleboards(arm, climber); + Telemetry.initializeShuffleboards(arm, climber, intake, shooter, swerve); } SmartDashboard.putData(auto.getAutonomousChooser()); From 3dba20b282a295f5259d915c5314bd39f9f0db71 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 12:20:07 -0500 Subject: [PATCH 13/28] qm4 --- .../autos/{home.auto => amp.home.auto} | 6 ++--- .../deploy/pathplanner/autos/center.home.auto | 25 +++++++++++++++++++ .../deploy/pathplanner/autos/stage.home.auto | 25 +++++++++++++++++++ src/main/java/frc/robot/climber/Climber.java | 4 +-- .../frc/robot/intake/IntakeConstants.java | 2 +- .../java/frc/robot/odometry/Odometry.java | 2 +- .../java/frc/robot/swerve/DriveCommand.java | 22 +++++----------- .../frc/robot/swerve/SwerveConstants.java | 8 +++--- 8 files changed, 67 insertions(+), 27 deletions(-) rename src/main/deploy/pathplanner/autos/{home.auto => amp.home.auto} (85%) create mode 100644 src/main/deploy/pathplanner/autos/center.home.auto create mode 100644 src/main/deploy/pathplanner/autos/stage.home.auto diff --git a/src/main/deploy/pathplanner/autos/home.auto b/src/main/deploy/pathplanner/autos/amp.home.auto similarity index 85% rename from src/main/deploy/pathplanner/autos/home.auto rename to src/main/deploy/pathplanner/autos/amp.home.auto index e8893f0..d903830 100644 --- a/src/main/deploy/pathplanner/autos/home.auto +++ b/src/main/deploy/pathplanner/autos/amp.home.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 2, - "y": 2 + "x": 0.8, + "y": 6.63 }, - "rotation": 0 + "rotation": 60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/center.home.auto b/src/main/deploy/pathplanner/autos/center.home.auto new file mode 100644 index 0000000..aeb8e4b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/center.home.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.36, + "y": 5.56 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "home" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/stage.home.auto b/src/main/deploy/pathplanner/autos/stage.home.auto new file mode 100644 index 0000000..254117d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/stage.home.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.8108477755417065, + "y": 4.311854008323402 + }, + "rotation": -57.20046872738075 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "home" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/climber/Climber.java b/src/main/java/frc/robot/climber/Climber.java index 08a3700..8a134f5 100644 --- a/src/main/java/frc/robot/climber/Climber.java +++ b/src/main/java/frc/robot/climber/Climber.java @@ -30,8 +30,8 @@ public class Climber extends Subsystem { /** Creates a new instance of the climber subsystem. */ private Climber() { - westElevator = ClimberFactory.createElevator(new CAN(6), false); - eastElevator = ClimberFactory.createElevator(new CAN(7), false); + westElevator = new ElevatorIOSim(); // TODO + eastElevator = new ElevatorIOSim(); // TODO westElevator.configure(); eastElevator.configure(); diff --git a/src/main/java/frc/robot/intake/IntakeConstants.java b/src/main/java/frc/robot/intake/IntakeConstants.java index 1eb80db..e3d2870 100644 --- a/src/main/java/frc/robot/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/intake/IntakeConstants.java @@ -43,7 +43,7 @@ public static class PivotMotorConstants { public static final Rotation2d OUT_ANGLE = Rotation2d.fromDegrees(0); /** Pivot motor's tolerance. */ - public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(4.0); + public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(8.0); /** Maximum speed of the pivot in rotations per second. */ public static final double MAXIMUM_SPEED = 1; diff --git a/src/main/java/frc/robot/odometry/Odometry.java b/src/main/java/frc/robot/odometry/Odometry.java index f17bc53..8b17f21 100644 --- a/src/main/java/frc/robot/odometry/Odometry.java +++ b/src/main/java/frc/robot/odometry/Odometry.java @@ -135,7 +135,7 @@ public void setRotation(Rotation2d rotation) { * @return a command that zeroes the rotation of the robot. */ public Command tare() { - return Commands.runOnce(() -> setRotation(Rotation2d.fromDegrees(0))); + return Commands.runOnce(() -> gyroscope.setYaw(0.0)); } /** diff --git a/src/main/java/frc/robot/swerve/DriveCommand.java b/src/main/java/frc/robot/swerve/DriveCommand.java index 111ec8d..831015b 100644 --- a/src/main/java/frc/robot/swerve/DriveCommand.java +++ b/src/main/java/frc/robot/swerve/DriveCommand.java @@ -97,22 +97,12 @@ public void execute() { headingFeedback.calculate(poseHeading.getRotations(), headingSetpoint.position); } - ChassisSpeeds chassisSpeeds; - - if (request.isRobotCentric()) { - chassisSpeeds = - new ChassisSpeeds( - translationVelocityMetersPerSecond.getX(), - translationVelocityMetersPerSecond.getY(), - Units.rotationsToRadians(omegaRotationsPerSecond)); - } else { - chassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds( - translationVelocityMetersPerSecond.getX(), - translationVelocityMetersPerSecond.getY(), - Units.rotationsToRadians(omegaRotationsPerSecond), - poseHeading); - } + ChassisSpeeds chassisSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + translationVelocityMetersPerSecond.getX(), + translationVelocityMetersPerSecond.getY(), + Units.rotationsToRadians(omegaRotationsPerSecond), + poseHeading); chassisSpeeds.vxMetersPerSecond = MathUtil.clamp( diff --git a/src/main/java/frc/robot/swerve/SwerveConstants.java b/src/main/java/frc/robot/swerve/SwerveConstants.java index 87cdb90..0c0cee8 100644 --- a/src/main/java/frc/robot/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/swerve/SwerveConstants.java @@ -67,28 +67,28 @@ public static class MK4iConstants { new SwerveModuleConfig( new SwerveModuleCAN(11, 10, 12, SWERVE_BUS), new Translation2d(X_OFFSET, Y_OFFSET), - Rotation2d.fromRotations(-0.304688)); + Rotation2d.fromRotations(-0.312256)); /** Module configuration for the north east swerve module. */ public static final SwerveModuleConfig NORTH_EAST_MODULE_CONFIG = new SwerveModuleConfig( new SwerveModuleCAN(2, 1, 3, SWERVE_BUS), new Translation2d(X_OFFSET, -Y_OFFSET), - Rotation2d.fromRotations(-0.433838)); + Rotation2d.fromRotations(-0.435547)); /** Module configuration for the south east swerve module. */ public static final SwerveModuleConfig SOUTH_EAST_MODULE_CONFIG = new SwerveModuleConfig( new SwerveModuleCAN(5, 4, 6, SWERVE_BUS), new Translation2d(-X_OFFSET, -Y_OFFSET), - Rotation2d.fromRotations(-0.104980)); + Rotation2d.fromRotations(-0.096436)); /** Module configuration for the south west swerve module. */ public static final SwerveModuleConfig SOUTH_WEST_MODULE_CONFIG = new SwerveModuleConfig( new SwerveModuleCAN(8, 7, 9, SWERVE_BUS), new Translation2d(-X_OFFSET, Y_OFFSET), - Rotation2d.fromRotations(-0.290771)); + Rotation2d.fromRotations(-0.309814)); /** * Calculates the maximum attainable open loop speed in meters per second. From 343859fb57083253b9248082dd3056bfe9b7a9c7 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 13:01:53 -0500 Subject: [PATCH 14/28] qm11 --- src/main/java/frc/robot/arm/ArmConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/arm/ArmConstants.java b/src/main/java/frc/robot/arm/ArmConstants.java index 98ea935..3b08ea5 100644 --- a/src/main/java/frc/robot/arm/ArmConstants.java +++ b/src/main/java/frc/robot/arm/ArmConstants.java @@ -88,7 +88,7 @@ public static class WristMotorConstants { public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0); /** Proportional gain in volts per rotation. */ - public static final double KP = 48.0; + public static final double KP = 36.0; /** Maximum speed of the wrist joint in rotations per second. */ public static final double MAXIMUM_SPEED = 1.2; From d15a51d44d03fe7bd583a24d886b73eb08905ac4 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 13:46:26 -0500 Subject: [PATCH 15/28] feat(auto): Add paths for leaving. --- .../autos/{amp.auto => amp.full.auto} | 0 .../pathplanner/autos/amp.leaveOnly.auto | 31 +++++++++++ .../autos/{center.auto => center.full.auto} | 0 .../pathplanner/autos/center.leaveOnly.auto | 31 +++++++++++ .../autos/{stage.auto => stage.full.auto} | 0 .../pathplanner/autos/stage.leaveOnly.auto | 31 +++++++++++ .../deploy/pathplanner/paths/amp.leave.path | 52 +++++++++++++++++++ .../pathplanner/paths/center.leave.path | 52 +++++++++++++++++++ .../deploy/pathplanner/paths/stage.leave.path | 52 +++++++++++++++++++ 9 files changed, 249 insertions(+) rename src/main/deploy/pathplanner/autos/{amp.auto => amp.full.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/amp.leaveOnly.auto rename src/main/deploy/pathplanner/autos/{center.auto => center.full.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/center.leaveOnly.auto rename src/main/deploy/pathplanner/autos/{stage.auto => stage.full.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/stage.leaveOnly.auto create mode 100644 src/main/deploy/pathplanner/paths/amp.leave.path create mode 100644 src/main/deploy/pathplanner/paths/center.leave.path create mode 100644 src/main/deploy/pathplanner/paths/stage.leave.path diff --git a/src/main/deploy/pathplanner/autos/amp.auto b/src/main/deploy/pathplanner/autos/amp.full.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/amp.auto rename to src/main/deploy/pathplanner/autos/amp.full.auto diff --git a/src/main/deploy/pathplanner/autos/amp.leaveOnly.auto b/src/main/deploy/pathplanner/autos/amp.leaveOnly.auto new file mode 100644 index 0000000..295a313 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/amp.leaveOnly.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.8, + "y": 6.63 + }, + "rotation": 60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "home" + } + }, + { + "type": "path", + "data": { + "pathName": "amp.leave" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/center.auto b/src/main/deploy/pathplanner/autos/center.full.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/center.auto rename to src/main/deploy/pathplanner/autos/center.full.auto diff --git a/src/main/deploy/pathplanner/autos/center.leaveOnly.auto b/src/main/deploy/pathplanner/autos/center.leaveOnly.auto new file mode 100644 index 0000000..a4a6808 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/center.leaveOnly.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.36, + "y": 5.56 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "home" + } + }, + { + "type": "path", + "data": { + "pathName": "center.leave" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/stage.auto b/src/main/deploy/pathplanner/autos/stage.full.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/stage.auto rename to src/main/deploy/pathplanner/autos/stage.full.auto diff --git a/src/main/deploy/pathplanner/autos/stage.leaveOnly.auto b/src/main/deploy/pathplanner/autos/stage.leaveOnly.auto new file mode 100644 index 0000000..3148f23 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/stage.leaveOnly.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 4.39 + }, + "rotation": -60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "home" + } + }, + { + "type": "path", + "data": { + "pathName": "stage.leave" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/amp.leave.path b/src/main/deploy/pathplanner/paths/amp.leave.path new file mode 100644 index 0000000..877aa20 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/amp.leave.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8009808327308976, + "y": 6.630585568863446 + }, + "prevControl": null, + "nextControl": { + "x": 1.0648639126144392, + "y": 7.087644470479496 + }, + "isLocked": false, + "linkedName": "amp.subwoofer" + }, + { + "anchor": { + "x": 5.448310896621795, + "y": 7.114065766593072 + }, + "prevControl": { + "x": 5.042684729899664, + "y": 7.114065766593072 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 60.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/center.leave.path b/src/main/deploy/pathplanner/paths/center.leave.path new file mode 100644 index 0000000..f4e336e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/center.leave.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3633965729469937, + "y": 5.555088802485298 + }, + "prevControl": null, + "nextControl": { + "x": 2.3633965729469937, + "y": 5.555088802485298 + }, + "isLocked": false, + "linkedName": "center.subwoofer" + }, + { + "anchor": { + "x": 5.4285770110001765, + "y": 6.640452511674254 + }, + "prevControl": { + "x": 4.4285770110001765, + "y": 6.640452511674254 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/stage.leave.path b/src/main/deploy/pathplanner/paths/stage.leave.path new file mode 100644 index 0000000..0ec2c59 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/stage.leave.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7636103146671326, + "y": 4.403274541213847 + }, + "prevControl": null, + "nextControl": { + "x": 0.8820136283968368, + "y": 4.2848712274841425 + }, + "isLocked": false, + "linkedName": "stage.subwoofer" + }, + { + "anchor": { + "x": 5.408843125378559, + "y": 1.0261620523241057 + }, + "prevControl": { + "x": 4.408843125378559, + "y": 1.0261620523241057 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -59.88626684901758, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file From 230b7b4b1e7e52802c3eb0bb5b6179386e99a52d Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 14:21:33 -0500 Subject: [PATCH 16/28] qm17 --- src/main/java/frc/robot/swerve/SwerveConstants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/swerve/SwerveConstants.java b/src/main/java/frc/robot/swerve/SwerveConstants.java index 0c0cee8..890cb51 100644 --- a/src/main/java/frc/robot/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/swerve/SwerveConstants.java @@ -67,21 +67,21 @@ public static class MK4iConstants { new SwerveModuleConfig( new SwerveModuleCAN(11, 10, 12, SWERVE_BUS), new Translation2d(X_OFFSET, Y_OFFSET), - Rotation2d.fromRotations(-0.312256)); + Rotation2d.fromRotations(-0.027100)); /** Module configuration for the north east swerve module. */ public static final SwerveModuleConfig NORTH_EAST_MODULE_CONFIG = new SwerveModuleConfig( new SwerveModuleCAN(2, 1, 3, SWERVE_BUS), new Translation2d(X_OFFSET, -Y_OFFSET), - Rotation2d.fromRotations(-0.435547)); + Rotation2d.fromRotations(-0.429688)); /** Module configuration for the south east swerve module. */ public static final SwerveModuleConfig SOUTH_EAST_MODULE_CONFIG = new SwerveModuleConfig( new SwerveModuleCAN(5, 4, 6, SWERVE_BUS), new Translation2d(-X_OFFSET, -Y_OFFSET), - Rotation2d.fromRotations(-0.096436)); + Rotation2d.fromRotations(-0.097168)); /** Module configuration for the south west swerve module. */ public static final SwerveModuleConfig SOUTH_WEST_MODULE_CONFIG = From cbb9a09f0339b9fa5abf931b677fa568c80c7ffc Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 15:20:46 -0500 Subject: [PATCH 17/28] fix(swerve): Decrease current limit. --- src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java index 8007c1f..002aca4 100644 --- a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java +++ b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java @@ -37,7 +37,7 @@ public DriveMotorIOTalonFX(CAN driveMotorCAN) { // TODO Copied from Citrus Circuits 2023 release talonFXBaseConfig.CurrentLimits = - new MotorCurrentLimits(300.0, 30.0, 60.0, 1.0).asCurrentLimitsConfigs(); + new MotorCurrentLimits(100.0, 30.0, 60.0, 1.0).asCurrentLimitsConfigs(); positionRotations = talonFX.getPosition(); velocityRotationsPerSecond = talonFX.getVelocity(); From d46dfa5af6e5bde8910bf409a321d0c95792eea8 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 15:21:03 -0500 Subject: [PATCH 18/28] feat(auto): Add backout path. --- src/main/deploy/pathplanner/autos/source.auto | 31 +++++++++++ .../pathplanner/paths/source.backout.path | 52 +++++++++++++++++++ 2 files changed, 83 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/source.auto create mode 100644 src/main/deploy/pathplanner/paths/source.backout.path diff --git a/src/main/deploy/pathplanner/autos/source.auto b/src/main/deploy/pathplanner/autos/source.auto new file mode 100644 index 0000000..9cc6469 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/source.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.62, + "y": 2.27 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "home" + } + }, + { + "type": "path", + "data": { + "pathName": "source.backout" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/source.backout.path b/src/main/deploy/pathplanner/paths/source.backout.path new file mode 100644 index 0000000..9f091a0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/source.backout.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.62, + "y": 2.2719022898816124 + }, + "prevControl": null, + "nextControl": { + "x": 1.6199999999999997, + "y": 2.2719022898816124 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.733583903687064, + "y": 1.475565404768469 + }, + "prevControl": { + "x": 3.2345968258270292, + "y": 1.6395171164094098 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file From 2da50a4c187ea56b76300940232a0f492ff87b6d Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 15:28:10 -0500 Subject: [PATCH 19/28] fix(swerve): Don't flip on red. --- src/main/java/frc/robot/swerve/DriveRequest.java | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/main/java/frc/robot/swerve/DriveRequest.java b/src/main/java/frc/robot/swerve/DriveRequest.java index fff747e..2605ea9 100644 --- a/src/main/java/frc/robot/swerve/DriveRequest.java +++ b/src/main/java/frc/robot/swerve/DriveRequest.java @@ -4,7 +4,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.lib.AllianceFlipHelper; public record DriveRequest( DriveRequest.TranslationMode translationMode, @@ -41,19 +40,11 @@ public static DriveRequest fromController(CommandXboxController controller) { double translationX = -controller.getLeftY(); - if (AllianceFlipHelper.shouldFlip()) { - translationX *= -1; - } - double translationY = -controller.getLeftX(); double translationMagnitude = Math.hypot(translationX, translationY); Rotation2d translationDirection = new Rotation2d(translationX, translationY); - if (AllianceFlipHelper.shouldFlip()) { - translationDirection.plus(Rotation2d.fromDegrees(180)); - } - translationMagnitude = MathUtil.applyDeadband(translationMagnitude, 0.1); translationMagnitude = Math.copySign(translationMagnitude * translationMagnitude, translationMagnitude); From 298ade6d09860a35e12419760d6b754a9e5ecb88 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 16:16:45 -0500 Subject: [PATCH 20/28] fix(swerve): Drop current limits. --- src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java | 3 +-- src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java index 002aca4..8cd30ff 100644 --- a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java +++ b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java @@ -35,9 +35,8 @@ public DriveMotorIOTalonFX(CAN driveMotorCAN) { talonFXBaseConfig.Feedback.SensorToMechanismRatio = MK4iConstants.DRIVE_GEARING; - // TODO Copied from Citrus Circuits 2023 release talonFXBaseConfig.CurrentLimits = - new MotorCurrentLimits(100.0, 30.0, 60.0, 1.0).asCurrentLimitsConfigs(); + new MotorCurrentLimits(60.0, 30.0, 60.0, 0.1).asCurrentLimitsConfigs(); positionRotations = talonFX.getPosition(); velocityRotationsPerSecond = talonFX.getVelocity(); diff --git a/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java b/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java index 44e48b1..ed1f07f 100644 --- a/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java +++ b/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java @@ -43,7 +43,7 @@ public void configure() { talonFXPIDFConfig.ClosedLoopGeneral.ContinuousWrap = true; talonFXPIDFConfig.CurrentLimits = - new MotorCurrentLimits(0.0, 20.0, 30.0, 1.0).asCurrentLimitsConfigs(); + new MotorCurrentLimits(40.0, 20.0, 30.0, 0.1).asCurrentLimitsConfigs(); talonFXPIDFConfig.Feedback.SensorToMechanismRatio = MK4iConstants.STEER_GEARING; From c10ebd78d86e920097add4f128bc7cb488c003cb Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 16:49:10 -0500 Subject: [PATCH 21/28] chore: Format. --- src/main/java/frc/robot/RobotConstants.java | 9 ++++++++- src/main/java/frc/robot/RobotContainer.java | 4 +++- src/main/java/frc/robot/climber/Climber.java | 1 - 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotConstants.java b/src/main/java/frc/robot/RobotConstants.java index 6e68847..f11c6e3 100644 --- a/src/main/java/frc/robot/RobotConstants.java +++ b/src/main/java/frc/robot/RobotConstants.java @@ -44,7 +44,14 @@ public enum Subsystem { SWERVE, } - public static final Set ALL_SUBSYSTEMS = EnumSet.of(Subsystem.ARM, Subsystem.CLIMBER, Subsystem.INTAKE, Subsystem.ODOMETRY, Subsystem.SHOOTER, Subsystem.SWERVE); + public static final Set ALL_SUBSYSTEMS = + EnumSet.of( + Subsystem.ARM, + Subsystem.CLIMBER, + Subsystem.INTAKE, + Subsystem.ODOMETRY, + Subsystem.SHOOTER, + Subsystem.SWERVE); public static final Set REAL_SUBSYSTEMS = ALL_SUBSYSTEMS; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6f600d7..9cf8d7a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -94,7 +94,9 @@ private void configureBindings() { operatorController.rightBumper().whileTrue(shooter.serialize()); operatorController.povUp().whileTrue(climber.up()); - operatorController.povDown().whileTrue(Commands.parallel(auto.readyIntake().repeatedly(), climber.down())); + operatorController + .povDown() + .whileTrue(Commands.parallel(auto.readyIntake().repeatedly(), climber.down())); operatorController.a().whileTrue(arm.amp()).whileFalse(auto.stow()); } diff --git a/src/main/java/frc/robot/climber/Climber.java b/src/main/java/frc/robot/climber/Climber.java index 8a134f5..24297e3 100644 --- a/src/main/java/frc/robot/climber/Climber.java +++ b/src/main/java/frc/robot/climber/Climber.java @@ -4,7 +4,6 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import frc.lib.CAN; import frc.lib.Subsystem; import frc.lib.Telemetry; import frc.robot.climber.ClimberConstants.ElevatorConstants; From 40230549c021708e49c3dd3607b6a00f60bb2c4c Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 17:13:36 -0500 Subject: [PATCH 22/28] qm36 --- src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java index 8cd30ff..a48f30e 100644 --- a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java +++ b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java @@ -36,7 +36,7 @@ public DriveMotorIOTalonFX(CAN driveMotorCAN) { talonFXBaseConfig.Feedback.SensorToMechanismRatio = MK4iConstants.DRIVE_GEARING; talonFXBaseConfig.CurrentLimits = - new MotorCurrentLimits(60.0, 30.0, 60.0, 0.1).asCurrentLimitsConfigs(); + new MotorCurrentLimits(80.0, 30.0, 60.0, 0.1).asCurrentLimitsConfigs(); positionRotations = talonFX.getPosition(); velocityRotationsPerSecond = talonFX.getVelocity(); From 11b096ec91525b20892f252c3e9634cb9d88a924 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 2 Mar 2024 20:06:32 -0500 Subject: [PATCH 23/28] WIP --- .../pathplanner/autos/amp.shootLeave.auto | 37 +++++++++++++++++++ src/main/java/frc/robot/arm/Arm.java | 29 ++++++++------- src/main/java/frc/robot/auto/Auto.java | 2 +- 3 files changed, 54 insertions(+), 14 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/amp.shootLeave.auto diff --git a/src/main/deploy/pathplanner/autos/amp.shootLeave.auto b/src/main/deploy/pathplanner/autos/amp.shootLeave.auto new file mode 100644 index 0000000..c94976a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/amp.shootLeave.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.8, + "y": 6.63 + }, + "rotation": 60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "home" + } + }, + { + "type": "named", + "data": { + "name": "shootNote" + } + }, + { + "type": "path", + "data": { + "pathName": "amp.leave" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index b4aa208..5d6fc3d 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.Subsystem; import frc.lib.Telemetry; import frc.robot.arm.LimitSwitchIO.LimitSwitchIOValues; @@ -76,15 +77,15 @@ public void periodic() { shoulderMotor.update(shoulderMotorValues); wristMotor.update(wristMotorValues); - if (limitSwitchValues.isPressed) { - setPosition(getPosition().withShoulder(ArmState.STOW.shoulder())); - } - setSetpoint(setpoint.nextSetpoint(goal)); } @Override public void addToShuffleboard(ShuffleboardTab tab) { + ShuffleboardLayout commands = Telemetry.addColumn(tab, "Commands"); + + commands.addString("Running Command", () -> this.getCurrentCommand() != null ? this.getCurrentCommand().getName() : "no running command"); + ShuffleboardLayout limitSwitch = Telemetry.addColumn(tab, "Limit Switch"); limitSwitch.addBoolean("Is Pressed?", () -> limitSwitchValues.isPressed); @@ -186,7 +187,7 @@ private MoveShoulderCommand moveShoulder(ArmState goal) { return new MoveShoulderCommand(goal); } - private MoveWristCommand moveWrist(ArmState goal) { + public MoveWristCommand moveWrist(ArmState goal) { return new MoveWristCommand(goal); } @@ -203,13 +204,15 @@ public Command amp() { } public Command home() { - return moveWrist(ArmState.STOW) - .andThen(moveShoulder(ArmState.STOW).until(() -> limitSwitchValues.isPressed)) - .finallyDo( - interrupted -> { - if (!interrupted) { - setPosition(ArmState.STOW); - } - }); + return Commands.race( + moveWrist(ArmState.STOW) + .andThen(moveShoulder(ArmState.STOW).until(() -> limitSwitchValues.isPressed)) + .finallyDo( + interrupted -> { + if (!interrupted) { + setPosition(ArmState.STOW); + } + }), + Commands.waitSeconds(2.5)); } } diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index 7685693..f9871e5 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -148,7 +148,7 @@ public Command stow() { public Command readyShoot() { return Commands.parallel( - Commands.waitUntil(intake::isNotStowed).andThen(arm.moveShoulderThenWrist(ArmState.SHOOT)), + Commands.waitUntil(intake::isNotStowed).andThen(arm.moveWrist(ArmState.SHOOT)), intake.out()); } From c16ca4804f6a9cb68745bdfde4c5696b21e35ce7 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sun, 3 Mar 2024 08:26:45 -0500 Subject: [PATCH 24/28] fix: Add timeouts, --- src/main/java/frc/robot/arm/Arm.java | 27 +++++++++++--------- src/main/java/frc/robot/auto/Auto.java | 15 ++++++++--- src/main/java/frc/robot/intake/Intake.java | 4 +-- src/main/java/frc/robot/shooter/Shooter.java | 5 ++-- 4 files changed, 30 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index 5d6fc3d..2b729cc 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.Subsystem; import frc.lib.Telemetry; import frc.robot.arm.LimitSwitchIO.LimitSwitchIOValues; @@ -84,7 +83,12 @@ public void periodic() { public void addToShuffleboard(ShuffleboardTab tab) { ShuffleboardLayout commands = Telemetry.addColumn(tab, "Commands"); - commands.addString("Running Command", () -> this.getCurrentCommand() != null ? this.getCurrentCommand().getName() : "no running command"); + commands.addString( + "Running Command", + () -> + this.getCurrentCommand() != null + ? this.getCurrentCommand().getName() + : "no running command"); ShuffleboardLayout limitSwitch = Telemetry.addColumn(tab, "Limit Switch"); @@ -204,15 +208,14 @@ public Command amp() { } public Command home() { - return Commands.race( - moveWrist(ArmState.STOW) - .andThen(moveShoulder(ArmState.STOW).until(() -> limitSwitchValues.isPressed)) - .finallyDo( - interrupted -> { - if (!interrupted) { - setPosition(ArmState.STOW); - } - }), - Commands.waitSeconds(2.5)); + return moveWrist(ArmState.STOW) + .andThen(moveShoulder(ArmState.STOW).until(() -> limitSwitchValues.isPressed)) + .finallyDo( + interrupted -> { + if (!interrupted) { + setPosition(ArmState.STOW); + } + }) + .withTimeout(3.0); } } diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index f9871e5..f10d469 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -131,9 +131,13 @@ public SendableChooser getAutonomousChooser() { } public Command readyIntake() { + double seconds = 4.0; + return Commands.parallel( - Commands.waitUntil(intake::isNotStowed).andThen(arm.moveShoulderThenWrist(ArmState.INTAKE)), - intake.out()); + Commands.waitUntil(intake::isNotStowed) + .andThen(arm.moveShoulderThenWrist(ArmState.INTAKE)), + intake.out()) + .withTimeout(seconds); } public Command intakeNote() { @@ -147,9 +151,12 @@ public Command stow() { } public Command readyShoot() { + double seconds = 4.0; + return Commands.parallel( - Commands.waitUntil(intake::isNotStowed).andThen(arm.moveWrist(ArmState.SHOOT)), - intake.out()); + Commands.waitUntil(intake::isNotStowed).andThen(arm.moveWrist(ArmState.SHOOT)), + intake.out()) + .withTimeout(seconds); } public Command shootNote() { diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index a4b5301..90b1070 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -140,11 +140,11 @@ private boolean atPivotSetpoint() { } public Command out() { - return Commands.race(pivotTo(PivotMotorConstants.MINIMUM_ANGLE), Commands.waitSeconds(2.0)); + return pivotTo(PivotMotorConstants.MINIMUM_ANGLE).withTimeout(3.0); } public Command in() { - return pivotTo(PivotMotorConstants.MAXIMUM_ANGLE); + return pivotTo(PivotMotorConstants.MAXIMUM_ANGLE).withTimeout(3.0); } private Command pivotTo(Rotation2d angle) { diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index ee0b5b4..422a1d0 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -10,7 +10,6 @@ import frc.robot.shooter.SerializerMotorIO.SerializerMotorIOValues; import frc.robot.shooter.ShooterConstants.FlywheelConstants; import frc.robot.shooter.ShooterConstants.SerializerConstants; -import frc.robot.shooter.ShooterConstants.ShooterCommandConstants; /** Subsystem class for the shooter subsystem. */ public class Shooter extends Subsystem { @@ -122,7 +121,7 @@ public Command spin() { * @return a command that shoots a note. */ public Command autoShoot() { - return Commands.deadline( - Commands.waitSeconds(ShooterCommandConstants.PRE_SHOOT_DELAY).andThen(serialize()), spin()); + return Commands.parallel(Commands.waitSeconds(1.0).andThen(serialize()), spin()) + .withTimeout(3.0); } } From 098a7b1b9bd33347f91781797719bb0bb791213f Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sun, 3 Mar 2024 09:20:45 -0500 Subject: [PATCH 25/28] qmXX --- simgui.json | 14 +++++++-- .../deploy/pathplanner/autos/amp.full.auto | 29 ++++--------------- .../pathplanner/paths/amp.toNearest.path | 12 ++++---- .../pathplanner/paths/amp.toSubwoofer.path | 10 +++---- src/main/java/frc/robot/auto/Auto.java | 20 ++++++------- 5 files changed, 37 insertions(+), 48 deletions(-) diff --git a/simgui.json b/simgui.json index 7742a03..d725fe9 100644 --- a/simgui.json +++ b/simgui.json @@ -23,6 +23,11 @@ "window": { "visible": true } + }, + "/SmartDashboard/SendableChooser[0]": { + "window": { + "visible": true + } } } }, @@ -36,9 +41,13 @@ "Feedforward": { "open": true }, + "Position": { + "open": true + }, "Velocities": { "open": true - } + }, + "open": true }, "Climber": { "East": { @@ -52,8 +61,7 @@ }, "West": { "open": true - }, - "open": true + } }, "Intake": { "Roller": { diff --git a/src/main/deploy/pathplanner/autos/amp.full.auto b/src/main/deploy/pathplanner/autos/amp.full.auto index c456e87..d579d60 100644 --- a/src/main/deploy/pathplanner/autos/amp.full.auto +++ b/src/main/deploy/pathplanner/autos/amp.full.auto @@ -17,12 +17,6 @@ "name": "home" } }, - { - "type": "named", - "data": { - "name": "readyShoot" - } - }, { "type": "named", "data": { @@ -36,34 +30,21 @@ } }, { - "type": "race", + "type": "path", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "intakeNote" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] + "pathName": "amp.toSubwoofer" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "amp.toSubwoofer" + "name": "shootNote" } }, { "type": "named", "data": { - "name": "shootNote" + "name": null } } ] diff --git a/src/main/deploy/pathplanner/paths/amp.toNearest.path b/src/main/deploy/pathplanner/paths/amp.toNearest.path index c29a804..5956295 100644 --- a/src/main/deploy/pathplanner/paths/amp.toNearest.path +++ b/src/main/deploy/pathplanner/paths/amp.toNearest.path @@ -16,15 +16,15 @@ }, { "anchor": { - "x": 2.3, - "y": 7.0 + "x": 3.188780992946603, + "y": 7.025263281295794 }, "prevControl": { - "x": 1.8943738332778692, - "y": 7.0 + "x": 2.7831548262244725, + "y": 7.025263281295794 }, "nextControl": null, - "isLocked": true, + "isLocked": false, "linkedName": "amp.nearestNote" } ], @@ -61,7 +61,7 @@ }, { "name": "New Event Marker", - "waypointRelativePos": 0.7, + "waypointRelativePos": 0.5, "command": { "type": "sequential", "data": { diff --git a/src/main/deploy/pathplanner/paths/amp.toSubwoofer.path b/src/main/deploy/pathplanner/paths/amp.toSubwoofer.path index ce24fcc..d1f8562 100644 --- a/src/main/deploy/pathplanner/paths/amp.toSubwoofer.path +++ b/src/main/deploy/pathplanner/paths/amp.toSubwoofer.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.3, - "y": 7.0 + "x": 3.188780992946603, + "y": 7.025263281295794 }, "prevControl": null, "nextControl": { - "x": 1.7778081710009588, - "y": 7.114065766593072 + "x": 2.6665891639475623, + "y": 7.139329047888865 }, "isLocked": false, "linkedName": "amp.nearestNote" @@ -39,7 +39,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.1, + "waypointRelativePos": 0.2, "command": { "type": "sequential", "data": { diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index f10d469..c8c0057 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -83,6 +83,7 @@ private Auto() { swerve); NamedCommands.registerCommand("home", Arm.getInstance().home()); + NamedCommands.registerCommand("stow", stow()); NamedCommands.registerCommand("readyIntake", readyIntake()); NamedCommands.registerCommand("intakeNote", intakeNote()); NamedCommands.registerCommand("readyShoot", readyShoot()); @@ -131,13 +132,11 @@ public SendableChooser getAutonomousChooser() { } public Command readyIntake() { - double seconds = 4.0; + double seconds = 3.0; return Commands.parallel( - Commands.waitUntil(intake::isNotStowed) - .andThen(arm.moveShoulderThenWrist(ArmState.INTAKE)), - intake.out()) - .withTimeout(seconds); + Commands.waitUntil(intake::isNotStowed).andThen(arm.moveShoulderThenWrist(ArmState.INTAKE)), + intake.out()).withTimeout(seconds); } public Command intakeNote() { @@ -145,18 +144,19 @@ public Command intakeNote() { } public Command stow() { + double seconds = 2.0; + return Commands.parallel( arm.moveWristThenShoulder(ArmState.STOW), - Commands.waitUntil(() -> arm.getPosition().at(ArmState.STOW)).andThen(intake.in())); + Commands.waitUntil(() -> arm.getPosition().at(ArmState.STOW)).withTimeout(2.0).andThen(intake.in())).withTimeout(seconds); } public Command readyShoot() { - double seconds = 4.0; + double seconds = 3.0; return Commands.parallel( - Commands.waitUntil(intake::isNotStowed).andThen(arm.moveWrist(ArmState.SHOOT)), - intake.out()) - .withTimeout(seconds); + Commands.waitUntil(intake::isNotStowed).andThen(arm.moveWrist(ArmState.SHOOT)), + intake.out()).withTimeout(seconds); } public Command shootNote() { From 6e7025b5a812d063cc264addd2f132d7f9d5c86d Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sun, 3 Mar 2024 13:12:20 -0500 Subject: [PATCH 26/28] p1 --- .../deploy/pathplanner/autos/!!!AMP!!!.auto | 37 +++++++++++++++++++ .../pathplanner/autos/amp.shootLeave.auto | 6 +++ src/main/java/frc/robot/arm/ArmState.java | 4 +- .../frc/robot/arm/WristMotorIOSparkMax.java | 3 +- .../shooter/SerializerMotorIOSparkMax.java | 2 +- src/main/java/frc/robot/shooter/Shooter.java | 6 ++- 6 files changed, 53 insertions(+), 5 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/!!!AMP!!!.auto diff --git a/src/main/deploy/pathplanner/autos/!!!AMP!!!.auto b/src/main/deploy/pathplanner/autos/!!!AMP!!!.auto new file mode 100644 index 0000000..185323b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/!!!AMP!!!.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.8, + "y": 6.63 + }, + "rotation": 60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "home" + } + }, + { + "type": "named", + "data": { + "name": "shootNote" + } + }, + { + "type": "named", + "data": { + "name": "stow" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/amp.shootLeave.auto b/src/main/deploy/pathplanner/autos/amp.shootLeave.auto index c94976a..8572861 100644 --- a/src/main/deploy/pathplanner/autos/amp.shootLeave.auto +++ b/src/main/deploy/pathplanner/autos/amp.shootLeave.auto @@ -23,6 +23,12 @@ "name": "shootNote" } }, + { + "type": "named", + "data": { + "name": "stow" + } + }, { "type": "path", "data": { diff --git a/src/main/java/frc/robot/arm/ArmState.java b/src/main/java/frc/robot/arm/ArmState.java index c35d202..25ff50b 100644 --- a/src/main/java/frc/robot/arm/ArmState.java +++ b/src/main/java/frc/robot/arm/ArmState.java @@ -16,8 +16,10 @@ public record ArmState(State shoulder, State wrist) { public static final ArmState STOW = new ArmState(ShoulderMotorConstants.MINIMUM_ANGLE, WristMotorConstants.MAXIMUM_ANGLE); + public static final ArmState SHOOT = STOW.withWrist(Rotation2d.fromDegrees(23.265)); - public static final ArmState INTAKE = STOW.withWrist(Rotation2d.fromDegrees(6.81)); + + public static final ArmState INTAKE = STOW.withWrist(Rotation2d.fromDegrees(0)); public static final ArmState AMP = new ArmState(ShoulderMotorConstants.MAXIMUM_ANGLE, Rotation2d.fromDegrees(0)); diff --git a/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java b/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java index f782063..4f9c1ec 100644 --- a/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java +++ b/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java @@ -23,7 +23,8 @@ public class WristMotorIOSparkMax implements WristMotorIO { private final AccelerationCalculator accelerationCalculator; public WristMotorIOSparkMax() { - sparkMax = new CANSparkMax(WristMotorConstants.CAN.id(), MotorType.kBrushless); + // TODO Temporary fix, using one of the climber Sparks + sparkMax = new CANSparkMax(6, MotorType.kBrushless); feedback = new PIDController(WristMotorConstants.KP, 0, 0); diff --git a/src/main/java/frc/robot/shooter/SerializerMotorIOSparkMax.java b/src/main/java/frc/robot/shooter/SerializerMotorIOSparkMax.java index 6d19f1b..81c1795 100644 --- a/src/main/java/frc/robot/shooter/SerializerMotorIOSparkMax.java +++ b/src/main/java/frc/robot/shooter/SerializerMotorIOSparkMax.java @@ -18,7 +18,7 @@ public SerializerMotorIOSparkMax() { public void configure() { Configurator.configureREV(sparkMax::restoreFactoryDefaults); - Configurator.configureREV(() -> sparkMax.setSmartCurrentLimit(30)); + Configurator.configureREV(() -> sparkMax.setSmartCurrentLimit(40)); Configurator.configureStatusFrames(sparkMax); } diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index 422a1d0..742a630 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -121,7 +121,9 @@ public Command spin() { * @return a command that shoots a note. */ public Command autoShoot() { - return Commands.parallel(Commands.waitSeconds(1.0).andThen(serialize()), spin()) - .withTimeout(3.0); + return Commands.parallel( + serialize().beforeStarting(Commands.waitSeconds(1.0)), + spin() + ).withTimeout(3.0); } } From 904860fd14caba4adc0988a25ac84f2207354118 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sun, 3 Mar 2024 15:18:12 -0500 Subject: [PATCH 27/28] add odo flip --- src/main/java/frc/robot/odometry/Odometry.java | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/odometry/Odometry.java b/src/main/java/frc/robot/odometry/Odometry.java index 8b17f21..ad1a451 100644 --- a/src/main/java/frc/robot/odometry/Odometry.java +++ b/src/main/java/frc/robot/odometry/Odometry.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.Subsystem; import frc.lib.Telemetry; +import frc.lib.AllianceFlipHelper; import frc.robot.odometry.GyroscopeIO.GyroscopeIOValues; import frc.robot.swerve.Swerve; import java.util.function.Supplier; @@ -135,7 +136,14 @@ public void setRotation(Rotation2d rotation) { * @return a command that zeroes the rotation of the robot. */ public Command tare() { - return Commands.runOnce(() -> gyroscope.setYaw(0.0)); + return Commands.runOnce(() -> { + // if (AllianceFlipHelper.shouldFlip()) { + // gyroscope.setYaw(0.5); + // } else { + // gyroscope.setYaw(0.0); + // } + gyroscope.setYaw(0.0); + }); } /** From 8a2c16d1af7d84fcf02635bedaef6ab02d5022aa Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sun, 3 Mar 2024 15:26:37 -0500 Subject: [PATCH 28/28] new wrist intake angle --- src/main/java/frc/robot/arm/ArmState.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/arm/ArmState.java b/src/main/java/frc/robot/arm/ArmState.java index 25ff50b..6e88117 100644 --- a/src/main/java/frc/robot/arm/ArmState.java +++ b/src/main/java/frc/robot/arm/ArmState.java @@ -17,9 +17,9 @@ public record ArmState(State shoulder, State wrist) { public static final ArmState STOW = new ArmState(ShoulderMotorConstants.MINIMUM_ANGLE, WristMotorConstants.MAXIMUM_ANGLE); - public static final ArmState SHOOT = STOW.withWrist(Rotation2d.fromDegrees(23.265)); + public static final ArmState SHOOT = STOW.withWrist(Rotation2d.fromDegrees(18)); - public static final ArmState INTAKE = STOW.withWrist(Rotation2d.fromDegrees(0)); + public static final ArmState INTAKE = STOW.withWrist(Rotation2d.fromDegrees(4)); public static final ArmState AMP = new ArmState(ShoulderMotorConstants.MAXIMUM_ANGLE, Rotation2d.fromDegrees(0));