From b9f4d81a5f72b9c63fd7a696666fb3d422e4ba9b Mon Sep 17 00:00:00 2001 From: riley Date: Mon, 10 Feb 2025 18:09:55 -0800 Subject: [PATCH 01/19] add position limits for elevator --- .../java/frc/robot/subsystems/lifter/ElevatorIOHardware.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/lifter/ElevatorIOHardware.java b/src/main/java/frc/robot/subsystems/lifter/ElevatorIOHardware.java index 520d32d6..1c3c389d 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ElevatorIOHardware.java +++ b/src/main/java/frc/robot/subsystems/lifter/ElevatorIOHardware.java @@ -50,7 +50,10 @@ public void resetPosition() { } public void setVoltage(double value) { - m_elevatorMotor.setControl(new VoltageOut(value)); + if (this.getPosition() > ElevatorConstants.POSITION_LOWER_LIMIT && + this.getPosition() < ElevatorConstants.POSITION_UPPER_LIMIT) { + m_elevatorMotor.setControl(new VoltageOut(value)); + } } public void setVoltage(PositionVoltage request) { From 6b4d6b8d71738e8abf921577c29eadecd1dd229b Mon Sep 17 00:00:00 2001 From: riley Date: Mon, 10 Feb 2025 18:11:13 -0800 Subject: [PATCH 02/19] add manual control for all mechanisms commands are NOT binded to controller buttons --- .../robot/subsystems/collar/CollarSubsystem.java | 16 ++++++++++++++-- .../robot/subsystems/lifter/ArmSubsystem.java | 10 ++++++++++ .../subsystems/lifter/ElevatorSubsystem.java | 9 +++++++++ 3 files changed, 33 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java b/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java index 62234921..c385ee82 100644 --- a/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java +++ b/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java @@ -1,9 +1,13 @@ package frc.robot.subsystems.collar; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.RobotContainer.RobotState; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.StartEndCommand; + +import frc.robot.Constants.CollarConstants; +import frc.robot.RobotContainer.RobotState; public class CollarSubsystem extends SubsystemBase { private final CollarIOHardware m_IO = new CollarIOHardware(); @@ -16,4 +20,12 @@ public CollarSubsystem() { public Command getCommand(RobotState targetRobotState) { return new WaitCommand(0); } + + public Command runCollar() { + return new StartEndCommand( + () -> m_IO.setVoltage(CollarConstants.OUTTAKE_MOTOR_SPEED), + () -> m_IO.motorOff(), + this + ); + } } diff --git a/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java index 51ebbaf8..825eaa6d 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ArmConstants; @@ -33,6 +34,15 @@ public class ArmSubsystem extends SubsystemBase { public ArmSubsystem() {} // commands + public Command manualMoveCommand() { + return new StartEndCommand( + () -> m_IO.setVoltage(ArmConstants.MANUAL_MOVE_MOTOR_SPEED), + () -> m_IO.motorOff(), + this + ); + } + + public Command moveToCommand(double targetPosition) { return moveToCommand(new TrapezoidProfile.State(targetPosition, 0)); } diff --git a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java index e69739ea..d1502cda 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ElevatorConstants; @@ -33,6 +34,14 @@ public class ElevatorSubsystem extends SubsystemBase{ public ElevatorSubsystem() {} // commands + public Command manualMoveCommand() { + return new StartEndCommand( + () -> m_IO.setVoltage(ElevatorConstants.MANUAL_MOVE_MOTOR_SPEED), + () -> m_IO.motorOff(), + this + ); + } + public Command moveToCommand(double targetPosition) { return moveToCommand(new TrapezoidProfile.State(targetPosition, 0)); } From 037ec4b08868b0ff450da3a37a232a035b25a806 Mon Sep 17 00:00:00 2001 From: riley Date: Mon, 10 Feb 2025 18:11:23 -0800 Subject: [PATCH 03/19] add more constants --- src/main/java/frc/robot/Constants.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 891ec703..88b5158e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -55,6 +55,9 @@ public static class ElevatorConstants { public static final double MAX_SPEED = 0.0; public static final double SPEED_LOWER_LIMIT = 0.0; public static final double SPEED_UPPER_LIMIT = 0.0; + + public static final double POSITION_LOWER_LIMIT = 0.0; + public static final double POSITION_UPPER_LIMIT = 0.0; // not tuned public static final double kS = 0; @@ -69,6 +72,7 @@ public static class ElevatorConstants { public static final double kMaxV = 0; public static final double kMaxA = 0; + public static final double MANUAL_MOVE_MOTOR_SPEED = 0.0; } public static class ArmConstants { @@ -93,6 +97,8 @@ public static class ArmConstants { public static final double kMaxV = 0; public static final double kMaxA = 0; + + public static final double MANUAL_MOVE_MOTOR_SPEED = 0.0; } public static class CollarConstants { @@ -105,6 +111,8 @@ public static class CollarConstants { public static final double MAX_SPEED = 0.0; public static final double SPEED_LOWER_LIMIT = 0.0; public static final double SPEED_UPPER_LIMIT = 0.0; + + public static final double OUTTAKE_MOTOR_SPEED = 0.0; } public static class RampConstants { From 7b66ef3fea4d9a962a7158fa12c4c30c516bf093 Mon Sep 17 00:00:00 2001 From: riley Date: Mon, 10 Feb 2025 18:11:49 -0800 Subject: [PATCH 04/19] update get arm position --- src/main/java/frc/robot/subsystems/lifter/ArmIOHardware.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/lifter/ArmIOHardware.java b/src/main/java/frc/robot/subsystems/lifter/ArmIOHardware.java index b5130950..1150c299 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ArmIOHardware.java +++ b/src/main/java/frc/robot/subsystems/lifter/ArmIOHardware.java @@ -26,7 +26,7 @@ public ArmIOHardware() { // getters public double getPosition() { double angle = m_armMotor.getPosition().getValue().in(Revolutions); - return angle*ArmConstants.DEGREES_PER_ROTATION; + return angle*ArmConstants.DEGREES_PER_ROTATION/360; } public double getVelocity() { From b7e0706408866c46cf3cd33c412446249ea586eb Mon Sep 17 00:00:00 2001 From: aishahnazar Date: Sat, 15 Feb 2025 12:53:00 -0800 Subject: [PATCH 05/19] add bindings for open loop control set controller bindings for open loop control --- src/main/java/frc/robot/RobotContainer.java | 57 +++++++++++++------ .../subsystems/collar/CollarSubsystem.java | 2 +- .../robot/subsystems/lifter/ArmSubsystem.java | 7 +++ .../subsystems/lifter/ElevatorIOHardware.java | 2 +- .../subsystems/lifter/ElevatorSubsystem.java | 8 +++ 5 files changed, 58 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d10c4a60..e5c7f066 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -49,11 +49,15 @@ import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.VisionSubsystem; import frc.robot.subsystems.collar.CollarSubsystem; +import frc.robot.subsystems.lifter.ArmSubsystem; +import frc.robot.subsystems.lifter.ElevatorIOHardware; +import frc.robot.subsystems.lifter.ElevatorSubsystem; import frc.robot.subsystems.lifter.LifterCommandFactory; import frc.robot.subsystems.ramp.RampSubsystem; import frc.robot.commands.AutoAlignCommand; + public class RobotContainer { record JoystickVals(double x, double y) { } @@ -78,6 +82,9 @@ record JoystickVals(double x, double y) { } private final RampSubsystem m_ramp = new RampSubsystem(); private final LifterCommandFactory m_lifter = new LifterCommandFactory(); + private final ElevatorSubsystem m_elevator = new ElevatorSubsystem(); + + private final ArmSubsystem m_arm = new ArmSubsystem(); private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband @@ -153,31 +160,49 @@ private void configureBindings() { // set next state, change LED colors accordingly - copilotJoystick.leftTrigger().onTrue( - new ParallelCommandGroup( - new InstantCommand(() -> {nextState = RobotState.L4_CORAL;}), - m_ledSubsystem.runSolidRed())); - copilotJoystick.rightTrigger().onTrue( - new ParallelCommandGroup( - new InstantCommand(() -> {nextState = RobotState.L3_CORAL;}), - m_ledSubsystem.runSolidOrange())); - copilotJoystick.leftBumper().onTrue( - new ParallelCommandGroup( - new InstantCommand(() -> {nextState = RobotState.L2_CORAL;}), - m_ledSubsystem.runSolidYellow())); - copilotJoystick.rightBumper().onTrue( - new ParallelCommandGroup( - new InstantCommand(() -> {nextState = RobotState.L1_CORAL;}), - m_ledSubsystem.runSolidWhite())); + //copilotJoystick.leftTrigger().onTrue( + //new ParallelCommandGroup( + //new InstantCommand(() -> {nextState = RobotState.L4_CORAL;}), + //m_ledSubsystem.runSolidRed())); + + //copilotJoystick.rightTrigger().onTrue( + //new ParallelCommandGroup( + //new InstantCommand(() -> {nextState = RobotState.L3_CORAL;}), + //m_ledSubsystem.runSolidOrange())); + + // copilotJoystick.leftBumper().onTrue( + // new ParallelCommandGroup( + // new InstantCommand(() -> {nextState = RobotState.L2_CORAL;}), + // m_ledSubsystem.runSolidYellow())); + + // copilotJoystick.rightBumper().onTrue( + // new ParallelCommandGroup( + // new InstantCommand(() -> {nextState = RobotState.L1_CORAL;}), + // m_ledSubsystem.runSolidWhite())); + copilotJoystick.a().onTrue( new ParallelCommandGroup( new InstantCommand(() -> {nextState = RobotState.L3_ALGAE;}), m_ledSubsystem.runSolidPurple())); + copilotJoystick.y().onTrue( new ParallelCommandGroup( new InstantCommand(() -> {nextState = RobotState.L2_ALGAE;}), m_ledSubsystem.runSolidPink())); + //open loop control testing: + copilotJoystick.leftTrigger().whileTrue( + m_elevator.manualMoveCommand()); + + copilotJoystick.rightTrigger().whileTrue( + m_arm.manualMoveCommand()); + + copilotJoystick.leftBumper().whileTrue( + m_elevator.manualMoveBackwardCommand()); + + copilotJoystick.rightBumper().whileTrue( + m_arm.manualMoveBackwardCommand()); + // run command runSolidGreen continuously if robot isWithinTarget() m_vision.isWithinTargetTrigger().whileTrue(m_ledSubsystem.runSolidGreen()); diff --git a/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java b/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java index 2296883a..2679ce93 100644 --- a/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java +++ b/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java @@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.robot.Constants.CollarConstants; -import frc.robot.RobotContainer.RobotState; +import frc.robot.RobotState; public class CollarSubsystem extends SubsystemBase { private final CollarIOHardware m_IO = new CollarIOHardware(); diff --git a/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java index 72379226..7110a446 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java @@ -44,6 +44,13 @@ public Command manualMoveCommand() { this ); } + public Command manualMoveBackwardCommand() { + return new StartEndCommand( + () -> m_IO.setVoltage(-ArmConstants.MANUAL_MOVE_MOTOR_SPEED), + () -> m_IO.motorOff(), + this + ); + } public Command moveToCommand(double targetPosition) { diff --git a/src/main/java/frc/robot/subsystems/lifter/ElevatorIOHardware.java b/src/main/java/frc/robot/subsystems/lifter/ElevatorIOHardware.java index 4c2757f6..9052b492 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ElevatorIOHardware.java +++ b/src/main/java/frc/robot/subsystems/lifter/ElevatorIOHardware.java @@ -55,7 +55,7 @@ public void setVoltage(double value) { if (this.getPosition() > ElevatorConstants.POSITION_LOWER_LIMIT && this.getPosition() < ElevatorConstants.POSITION_UPPER_LIMIT) { m_elevatorMotor.setControl(new VoltageOut(value)); - } + } } public void requestClosedLoopPosition(double value) { diff --git a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java index 5c23ef5e..760710f9 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java @@ -44,6 +44,14 @@ public Command manualMoveCommand() { ); } + public Command manualMoveBackwardCommand() { + return new StartEndCommand( + () -> m_IO.setVoltage(-ElevatorConstants.MANUAL_MOVE_MOTOR_SPEED), + () -> m_IO.motorOff(), + this + ); + } + public Command moveToCommand(double targetPosition) { return moveToCommand(new TrapezoidProfile.State(targetPosition, 0)); } From fb571733f2d993b6433ed4dc89891028a48b7859 Mon Sep 17 00:00:00 2001 From: aishahnazar Date: Sat, 15 Feb 2025 12:59:02 -0800 Subject: [PATCH 06/19] add collar bindings add open loop control bindings for collar --- src/main/java/frc/robot/RobotContainer.java | 22 ++++++++++++------- .../subsystems/collar/CollarSubsystem.java | 8 +++++++ 2 files changed, 22 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e5c7f066..2c5d53d5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -180,15 +180,15 @@ private void configureBindings() { // new InstantCommand(() -> {nextState = RobotState.L1_CORAL;}), // m_ledSubsystem.runSolidWhite())); - copilotJoystick.a().onTrue( - new ParallelCommandGroup( - new InstantCommand(() -> {nextState = RobotState.L3_ALGAE;}), - m_ledSubsystem.runSolidPurple())); + // copilotJoystick.a().onTrue( + // new ParallelCommandGroup( + // new InstantCommand(() -> {nextState = RobotState.L3_ALGAE;}), + // m_ledSubsystem.runSolidPurple())); - copilotJoystick.y().onTrue( - new ParallelCommandGroup( - new InstantCommand(() -> {nextState = RobotState.L2_ALGAE;}), - m_ledSubsystem.runSolidPink())); + // copilotJoystick.y().onTrue( + // new ParallelCommandGroup( + // new InstantCommand(() -> {nextState = RobotState.L2_ALGAE;}), + // m_ledSubsystem.runSolidPink())); //open loop control testing: copilotJoystick.leftTrigger().whileTrue( @@ -202,6 +202,12 @@ private void configureBindings() { copilotJoystick.rightBumper().whileTrue( m_arm.manualMoveBackwardCommand()); + + copilotJoystick.a().whileTrue( + m_collar.runCollar()); + + copilotJoystick.y().whileTrue( + m_collar.runCollarBackward()); // run command runSolidGreen continuously if robot isWithinTarget() m_vision.isWithinTargetTrigger().whileTrue(m_ledSubsystem.runSolidGreen()); diff --git a/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java b/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java index 2679ce93..916e687f 100644 --- a/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java +++ b/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java @@ -28,4 +28,12 @@ public Command runCollar() { this ); } + + public Command runCollarBackward() { + return new StartEndCommand( + () -> m_IO.setVoltage(CollarConstants.OUTTAKE_MOTOR_SPEED), + () -> m_IO.motorOff(), + this + ); + } } From d8fc972790017b8f834c8bfacd08d85d6f14d71d Mon Sep 17 00:00:00 2001 From: amzoeee Date: Sat, 15 Feb 2025 15:00:03 -0800 Subject: [PATCH 07/19] fix runcollarbackward --- src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java b/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java index 916e687f..f0e5472e 100644 --- a/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java +++ b/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java @@ -31,7 +31,7 @@ public Command runCollar() { public Command runCollarBackward() { return new StartEndCommand( - () -> m_IO.setVoltage(CollarConstants.OUTTAKE_MOTOR_SPEED), + () -> m_IO.setVoltage(-CollarConstants.OUTTAKE_MOTOR_SPEED), () -> m_IO.motorOff(), this ); From 6adc9eda33674e3afa3f2fb281f4dcc96be0f23b Mon Sep 17 00:00:00 2001 From: amzoeee Date: Sat, 15 Feb 2025 15:14:34 -0800 Subject: [PATCH 08/19] put arm and elevator position and voltage to smart dashboard --- .../java/frc/robot/subsystems/lifter/ArmSubsystem.java | 7 +++++++ .../frc/robot/subsystems/lifter/ElevatorSubsystem.java | 7 +++++++ 2 files changed, 14 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java index 7110a446..e653627f 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; @@ -87,4 +88,10 @@ private void executeMoveTo() { m_IO.setVoltage(feedForwardPower + PIDPower); } + + @Override + public void periodic() { + SmartDashboard.putNumber("arm/position", m_IO.getPosition()); + SmartDashboard.putNumber("arm/velocity", m_IO.getVelocity()); + } } diff --git a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java index 760710f9..2c42cdd4 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; @@ -86,4 +87,10 @@ private void executeMoveTo() { m_IO.setVoltage(feedForwardPower + PIDPower); } + + @Override + public void periodic() { + SmartDashboard.putNumber("elevator/position", m_IO.getPosition()); + SmartDashboard.putNumber("elevator/velocity", m_IO.getVelocity()); + } } From 7adf950baa910089fdba4188070ee2a70f0c01ae Mon Sep 17 00:00:00 2001 From: riley Date: Sat, 15 Feb 2025 15:20:05 -0800 Subject: [PATCH 09/19] add keep in place command --- .../java/frc/robot/subsystems/lifter/ArmSubsystem.java | 8 +++++++- .../frc/robot/subsystems/lifter/ElevatorSubsystem.java | 7 +++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java index 7110a446..ceed9dee 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java @@ -37,6 +37,13 @@ public ArmSubsystem() { } // commands + public Command keepInPlaceCommand() { + return new StartEndCommand( + () -> m_IO.setVoltage(m_feedforward.calculate(m_IO.getPosition() - ArmConstants.POSITION_OFFSET, 0)), + () -> m_IO.motorOff() + ); + } + public Command manualMoveCommand() { return new StartEndCommand( () -> m_IO.setVoltage(ArmConstants.MANUAL_MOVE_MOTOR_SPEED), @@ -52,7 +59,6 @@ public Command manualMoveBackwardCommand() { ); } - public Command moveToCommand(double targetPosition) { return moveToCommand(new TrapezoidProfile.State(targetPosition, 0)); } diff --git a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java index 760710f9..6ab7bd27 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java @@ -36,6 +36,13 @@ public ElevatorSubsystem() { } // commands + public Command keepInPlaceCommand() { + return new StartEndCommand( + () -> m_IO.setVoltage(m_feedforward.calculate(m_IO.getPosition(), 0)), + () -> m_IO.motorOff() + ); + } + public Command manualMoveCommand() { return new StartEndCommand( () -> m_IO.setVoltage(ElevatorConstants.MANUAL_MOVE_MOTOR_SPEED), From b6ea85f29739e61d5b7f48388c4badca87ccca7f Mon Sep 17 00:00:00 2001 From: amzoeee Date: Sat, 15 Feb 2025 16:29:58 -0800 Subject: [PATCH 10/19] set anti-gravity default commands --- src/main/java/frc/robot/RobotContainer.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2c5d53d5..5cbf1b4e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -208,6 +208,10 @@ private void configureBindings() { copilotJoystick.y().whileTrue( m_collar.runCollarBackward()); + + m_elevator.setDefaultCommand(m_elevator.keepInPlaceCommand()); + m_arm.setDefaultCommand(m_arm.keepInPlaceCommand()); + // run command runSolidGreen continuously if robot isWithinTarget() m_vision.isWithinTargetTrigger().whileTrue(m_ledSubsystem.runSolidGreen()); From 069bf175a3a2e2bfab27cb9c747871473e80b41c Mon Sep 17 00:00:00 2001 From: amzoeee Date: Sat, 15 Feb 2025 16:35:39 -0800 Subject: [PATCH 11/19] require subsystem in keepinplace commands --- src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java | 3 ++- .../java/frc/robot/subsystems/lifter/ElevatorSubsystem.java | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java index 0ca6c5e7..4bb9a8b8 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java @@ -41,7 +41,8 @@ public ArmSubsystem() { public Command keepInPlaceCommand() { return new StartEndCommand( () -> m_IO.setVoltage(m_feedforward.calculate(m_IO.getPosition() - ArmConstants.POSITION_OFFSET, 0)), - () -> m_IO.motorOff() + () -> m_IO.motorOff(), + this ); } diff --git a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java index fd1369a3..b37b96b0 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java @@ -40,7 +40,8 @@ public ElevatorSubsystem() { public Command keepInPlaceCommand() { return new StartEndCommand( () -> m_IO.setVoltage(m_feedforward.calculate(m_IO.getPosition(), 0)), - () -> m_IO.motorOff() + () -> m_IO.motorOff(), + this ); } From d1d14c85c37919e9c03e0068bbc72fb27479ee5b Mon Sep 17 00:00:00 2001 From: amzoeee Date: Sat, 15 Feb 2025 18:16:41 -0800 Subject: [PATCH 12/19] change mechanism commands to runcommands --- .../robot/subsystems/collar/CollarSubsystem.java | 14 ++++++++++---- .../frc/robot/subsystems/lifter/ArmSubsystem.java | 12 +++++------- .../robot/subsystems/lifter/ElevatorSubsystem.java | 12 +++++------- 3 files changed, 20 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java b/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java index f0e5472e..0f963878 100644 --- a/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java +++ b/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; @@ -21,18 +22,23 @@ public Command getCommand(RobotState targetRobotState) { return new WaitCommand(0); } + public Command runCollarOff() { + return new RunCommand( + () -> m_IO.setVoltage(0), + this + ); + } + public Command runCollar() { - return new StartEndCommand( + return new RunCommand( () -> m_IO.setVoltage(CollarConstants.OUTTAKE_MOTOR_SPEED), - () -> m_IO.motorOff(), this ); } public Command runCollarBackward() { - return new StartEndCommand( + return new RunCommand( () -> m_IO.setVoltage(-CollarConstants.OUTTAKE_MOTOR_SPEED), - () -> m_IO.motorOff(), this ); } diff --git a/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java index 4bb9a8b8..e2b044b6 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -39,24 +40,21 @@ public ArmSubsystem() { // commands public Command keepInPlaceCommand() { - return new StartEndCommand( - () -> m_IO.setVoltage(m_feedforward.calculate(m_IO.getPosition() - ArmConstants.POSITION_OFFSET, 0)), - () -> m_IO.motorOff(), + return new RunCommand( + () -> m_IO.setVoltage(0), this ); } public Command manualMoveCommand() { - return new StartEndCommand( + return new RunCommand( () -> m_IO.setVoltage(ArmConstants.MANUAL_MOVE_MOTOR_SPEED), - () -> m_IO.motorOff(), this ); } public Command manualMoveBackwardCommand() { - return new StartEndCommand( + return new RunCommand( () -> m_IO.setVoltage(-ArmConstants.MANUAL_MOVE_MOTOR_SPEED), - () -> m_IO.motorOff(), this ); } diff --git a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java index b37b96b0..7fc7f0dd 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -38,25 +39,22 @@ public ElevatorSubsystem() { // commands public Command keepInPlaceCommand() { - return new StartEndCommand( - () -> m_IO.setVoltage(m_feedforward.calculate(m_IO.getPosition(), 0)), - () -> m_IO.motorOff(), + return new RunCommand( + () -> m_IO.setVoltage(ElevatorConstants.kG), this ); } public Command manualMoveCommand() { - return new StartEndCommand( + return new RunCommand( () -> m_IO.setVoltage(ElevatorConstants.MANUAL_MOVE_MOTOR_SPEED), - () -> m_IO.motorOff(), this ); } public Command manualMoveBackwardCommand() { - return new StartEndCommand( + return new RunCommand( () -> m_IO.setVoltage(-ElevatorConstants.MANUAL_MOVE_MOTOR_SPEED), - () -> m_IO.motorOff(), this ); } From eec626e882936884ca7b5b18e4cda82418022aaf Mon Sep 17 00:00:00 2001 From: amzoeee Date: Sat, 15 Feb 2025 18:16:50 -0800 Subject: [PATCH 13/19] add collar default command --- src/main/java/frc/robot/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5cbf1b4e..4afdc10e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -211,6 +211,8 @@ private void configureBindings() { m_elevator.setDefaultCommand(m_elevator.keepInPlaceCommand()); m_arm.setDefaultCommand(m_arm.keepInPlaceCommand()); + m_collar.setDefaultCommand(m_collar.runCollarOff()); + // run command runSolidGreen continuously if robot isWithinTarget() From 5aabf95d4d87af0278254586b1e34b5dbefe8c1c Mon Sep 17 00:00:00 2001 From: amzoeee Date: Sat, 15 Feb 2025 18:17:41 -0800 Subject: [PATCH 14/19] set robot reveal event mechnaism voltages --- src/main/java/frc/robot/Constants.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 79274285..7a2e6374 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.Units; /** @@ -73,7 +74,7 @@ public static class ElevatorConstants { public static final double kMaxV = 0; public static final double kMaxA = 0; - public static final double MANUAL_MOVE_MOTOR_SPEED = 0.0; + public static final double MANUAL_MOVE_MOTOR_SPEED = 2.0; } public static class ArmConstants { @@ -101,7 +102,7 @@ public static class ArmConstants { public static final double kMaxV = 0; public static final double kMaxA = 0; - public static final double MANUAL_MOVE_MOTOR_SPEED = 0.0; + public static final double MANUAL_MOVE_MOTOR_SPEED = 3.0; } public static class CollarConstants { @@ -115,7 +116,7 @@ public static class CollarConstants { public static final double SPEED_LOWER_LIMIT = 0.0; public static final double SPEED_UPPER_LIMIT = 0.0; - public static final double OUTTAKE_MOTOR_SPEED = 0.0; + public static final double OUTTAKE_MOTOR_SPEED = 4.0; } public static class RampConstants { From 05c4468be54301f68ba21ea14c4303500b0ded79 Mon Sep 17 00:00:00 2001 From: amzoeee Date: Sat, 15 Feb 2025 18:18:10 -0800 Subject: [PATCH 15/19] add elevator soft limits and kG --- src/main/java/frc/robot/Constants.java | 10 +++++----- .../subsystems/lifter/ElevatorIOHardware.java | 19 ++++++++++++++++--- 2 files changed, 21 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7a2e6374..135bab2f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -51,19 +51,19 @@ public static class ElevatorConstants { public static final int COUNTS_PER_REV = 42; // may need to be updated public static final double INITIAL_POSITION = 0; - public static final double METERS_PER_ROTATION = 0.0; + public static final double METERS_PER_ROTATION = Units.Meters.convertFrom(1.751 * Math.PI/5, Units.Inches); // temp public static final double MAX_SPEED = 0.0; public static final double SPEED_LOWER_LIMIT = 0.0; public static final double SPEED_UPPER_LIMIT = 0.0; - public static final double POSITION_LOWER_LIMIT = 0.0; - public static final double POSITION_UPPER_LIMIT = 0.0; + public static final double POSITION_LOWER_LIMIT = 0.01; + public static final double POSITION_UPPER_LIMIT = 0.71; public static final int MOTOR_STATOR_LIMIT = 60; // needs to be tuned // not tuned public static final double kS = 0; - public static final double kG = 0; + public static final double kG = 0.2; public static final double kV = 12/MAX_SPEED; // may need to be updated public static final double kA = 0; @@ -84,7 +84,7 @@ public static class ArmConstants { public static final double INITIAL_POSITION = 0; // facing down public static final double POSITION_OFFSET = Math.PI/2; // difference between pid 0 (horizontal) and our 0 (down) - public static final double DEGREES_PER_ROTATION = 0.0; + public static final double DEGREES_PER_ROTATION = 1; // temp public static final double MAX_SPEED = 0.0; public static final double SPEED_LOWER_LIMIT = 0.0; public static final double SPEED_UPPER_LIMIT = 0.0; diff --git a/src/main/java/frc/robot/subsystems/lifter/ElevatorIOHardware.java b/src/main/java/frc/robot/subsystems/lifter/ElevatorIOHardware.java index 9052b492..8089f4b1 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ElevatorIOHardware.java +++ b/src/main/java/frc/robot/subsystems/lifter/ElevatorIOHardware.java @@ -1,8 +1,11 @@ package frc.robot.subsystems.lifter; import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.StatusSignal; @@ -52,10 +55,20 @@ public void resetPosition() { } public void setVoltage(double value) { - if (this.getPosition() > ElevatorConstants.POSITION_LOWER_LIMIT && - this.getPosition() < ElevatorConstants.POSITION_UPPER_LIMIT) { - m_elevatorMotor.setControl(new VoltageOut(value)); + value = MathUtil.clamp(value, -3, 3); + + // if position is too low, only run the elevator up + if (this.getPosition() < ElevatorConstants.POSITION_LOWER_LIMIT) { + value = MathUtil.clamp(value, 0, 1000); } + // if position is too high, only run the elevator down (or in placd) + if (this.getPosition() > ElevatorConstants.POSITION_UPPER_LIMIT) { + value = MathUtil.clamp(value, -1000, ElevatorConstants.kG); + + } + + m_elevatorMotor.setControl(new VoltageOut(value)); + SmartDashboard.putNumber("elevator/voltage", value); } public void requestClosedLoopPosition(double value) { From 1ea4d6433479eb61ad39ea2047cd602a506f2984 Mon Sep 17 00:00:00 2001 From: amzoeee Date: Sat, 15 Feb 2025 18:18:45 -0800 Subject: [PATCH 16/19] use dynamene's tuner constants translation controlls are flipped on both axis for some reason. --- src/main/java/frc/robot/Controls.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 6 +- ...tants.java => TunerConstantsCeridwen.java} | 2 +- .../generated/TunerConstantsDynamene.java | 286 ++++++++++++++++++ .../subsystems/CommandSwerveDrivetrain.java | 3 +- 5 files changed, 292 insertions(+), 7 deletions(-) rename src/main/java/frc/robot/generated/{TunerConstants.java => TunerConstantsCeridwen.java} (99%) create mode 100644 src/main/java/frc/robot/generated/TunerConstantsDynamene.java diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index b176a1db..3f2aadc6 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -7,7 +7,7 @@ public class Controls { public static JoystickVals adjustInputs(double x, double y, boolean slowmode) { - return adjustSlowmode(inputShape(x, y), slowmode); + return adjustSlowmode(inputShape(-x, -y), slowmode); // negated x and y input for DYNAMENE } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4afdc10e..5308dada 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,7 +43,7 @@ import frc.robot.Constants.OperatorConstants; import frc.robot.commands.Autos; import frc.robot.commands.RotateToFaceReefCommand; -import frc.robot.generated.TunerConstants; +import frc.robot.generated.TunerConstantsDynamene; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.LEDSubsystem; @@ -65,7 +65,7 @@ record JoystickVals(double x, double y) { } private RobotState currentState = RobotState.INTAKE; private RobotState nextState = RobotState.INTAKE; - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12VoltsMps desired top speed *0.3 for pid tuning 9/15 + private double MaxSpeed = TunerConstantsDynamene.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12VoltsMps desired top speed *0.3 for pid tuning 9/15 private double MaxAngularRate = 1.5 * Math.PI; // 3/4 of a rotation per second max angular velocity /* Setting up bindings for necessary control of the swerve drive platform */ @@ -74,7 +74,7 @@ record JoystickVals(double x, double y) { } private final CommandXboxController testJoystick = new CommandXboxController(OperatorConstants.kTestControllerPort); // test joystick - private final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); // My drivetrain + private final CommandSwerveDrivetrain drivetrain = TunerConstantsDynamene.createDrivetrain(); // My drivetrain private final LEDSubsystem m_ledSubsystem = new LEDSubsystem(); private final VisionSubsystem m_vision = new VisionSubsystem(); private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstantsCeridwen.java similarity index 99% rename from src/main/java/frc/robot/generated/TunerConstants.java rename to src/main/java/frc/robot/generated/TunerConstantsCeridwen.java index 17650848..f4d96625 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstantsCeridwen.java @@ -20,7 +20,7 @@ // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html -public class TunerConstants { +public class TunerConstantsCeridwen { // Both sets of gains need to be tuned to your individual robot. // The steer motor uses any SwerveModule.SteerRequestType control request with the diff --git a/src/main/java/frc/robot/generated/TunerConstantsDynamene.java b/src/main/java/frc/robot/generated/TunerConstantsDynamene.java new file mode 100644 index 00000000..951f9c44 --- /dev/null +++ b/src/main/java/frc/robot/generated/TunerConstantsDynamene.java @@ -0,0 +1,286 @@ +package frc.robot.generated; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.hardware.*; +import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.swerve.*; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.*; + +import frc.robot.subsystems.CommandSwerveDrivetrain; + +// Generated by the Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +public class TunerConstantsDynamene { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(2.66).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.5714285714285716; + + private static final double kDriveGearRatio = 6.746031746031747; + private static final double kSteerGearRatio = 21.428571428571427; + private static final Distance kWheelRadius = Inches.of(2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 13; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + + // Front Left + private static final int kFrontLeftDriveMotorId = 2; + private static final int kFrontLeftSteerMotorId = 1; + private static final int kFrontLeftEncoderId = 3; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.284912109375); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10.375); + private static final Distance kFrontLeftYPos = Inches.of(10.375); + + // Front Right + private static final int kFrontRightDriveMotorId = 11; + private static final int kFrontRightSteerMotorId = 10; + private static final int kFrontRightEncoderId = 12; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.26171875); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10.375); + private static final Distance kFrontRightYPos = Inches.of(-10.375); + + // Back Left + private static final int kBackLeftDriveMotorId = 5; + private static final int kBackLeftSteerMotorId = 4; + private static final int kBackLeftEncoderId = 6; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.493408203125); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10.375); + private static final Distance kBackLeftYPos = Inches.of(10.375); + + // Back Right + private static final int kBackRightDriveMotorId = 8; + private static final int kBackRightSteerMotorId = 7; + private static final int kBackRightEncoderId = 9; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.03662109375); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10.375); + private static final Distance kBackRightYPos = Inches.of(-10.375); + + + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted + ); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted + ); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted + ); + public static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted + ); + + /** + * Creates a CommandSwerveDrivetrain instance. + * This should only be called once in your robot program,. + */ + public static CommandSwerveDrivetrain createDrivetrain() { + return new CommandSwerveDrivetrain( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + ); + } + + + /** + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules + ); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index fe0d251c..26a69ebc 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -41,9 +41,8 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.DrivetrainConstants; -import frc.robot.generated.TunerConstants; -import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.generated.TunerConstantsDynamene.TunerSwerveDrivetrain; /** From cb8c76b41763dcdfdb12d30cb543cb3d88a83cd9 Mon Sep 17 00:00:00 2001 From: amzoeee Date: Sun, 16 Feb 2025 15:24:28 -0800 Subject: [PATCH 17/19] unflip drivetrain teleop x y --- src/main/java/frc/robot/Controls.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 3f2aadc6..ddfdbd72 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -7,7 +7,7 @@ public class Controls { public static JoystickVals adjustInputs(double x, double y, boolean slowmode) { - return adjustSlowmode(inputShape(-x, -y), slowmode); // negated x and y input for DYNAMENE + return adjustSlowmode(inputShape(x, y), slowmode); } From 2b0d0bb29d62287de35a98bc24b2f47cde3bd3a7 Mon Sep 17 00:00:00 2001 From: aishahnazar Date: Mon, 17 Feb 2025 18:08:40 -0800 Subject: [PATCH 18/19] sysID routines implement sysID routines --- src/main/java/frc/robot/RobotContainer.java | 23 +++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5308dada..e35796e1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -231,6 +231,29 @@ private void configureBindings() { testJoystick.leftBumper().and(testJoystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); testJoystick.rightBumper().and(testJoystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); testJoystick.rightBumper().and(testJoystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); + + //sysID routines for mechanisms + //(comment out other mechanisms while running one of them since they are all bound + //to the same buttons ) + //or change the bindings buttons haha + //run sysID routines for collar + testJoystick.a().whileTrue(m_collar.sysIdDynamic(Direction.kForward)); + testJoystick.b().whileTrue(m_collar.sysIdDynamic(Direction.kReverse)); + testJoystick.x().whileTrue(m_collar.sysIdQuasistatic(Direction.kForward)); + testJoystick.y().whileTrue(m_collar.sysIdQuasistatic(Direction.kReverse)); + + //run sysID routines for elevator + testJoystick.a().whileTrue(m_elevator.sysIdDynamic(Direction.kForward)); + testJoystick.b().whileTrue(m_elevator.sysIdDynamic(Direction.kReverse)); + testJoystick.x().whileTrue(m_elevator.sysIdQuasistatic(Direction.kForward)); + testJoystick.y().whileTrue(m_elevator.sysIdQuasistatic(Direction.kReverse)); + + //run sysID routines for arm + testJoystick.a().whileTrue(m_arm.sysIdDynamic(Direction.kForward)); + testJoystick.b().whileTrue(m_arm.sysIdDynamic(Direction.kReverse)); + testJoystick.x().whileTrue(m_arm.sysIdQuasistatic(Direction.kForward)); + testJoystick.y().whileTrue(m_arm.sysIdQuasistatic(Direction.kReverse)); + if (Utils.isSimulation()) { drivetrain.resetPose(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); } From 58c89fdef52d64b6e6a1cfc8f4a11c1e16e74625 Mon Sep 17 00:00:00 2001 From: aishahnazar Date: Mon, 17 Feb 2025 19:35:40 -0800 Subject: [PATCH 19/19] add elevator sysID add code and bindings for elevator sysID --- src/main/java/frc/robot/RobotContainer.java | 10 ---- .../subsystems/lifter/ElevatorSubsystem.java | 55 ++++++++++++++++++- 2 files changed, 54 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e35796e1..5e315e54 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -237,10 +237,6 @@ private void configureBindings() { //to the same buttons ) //or change the bindings buttons haha //run sysID routines for collar - testJoystick.a().whileTrue(m_collar.sysIdDynamic(Direction.kForward)); - testJoystick.b().whileTrue(m_collar.sysIdDynamic(Direction.kReverse)); - testJoystick.x().whileTrue(m_collar.sysIdQuasistatic(Direction.kForward)); - testJoystick.y().whileTrue(m_collar.sysIdQuasistatic(Direction.kReverse)); //run sysID routines for elevator testJoystick.a().whileTrue(m_elevator.sysIdDynamic(Direction.kForward)); @@ -248,12 +244,6 @@ private void configureBindings() { testJoystick.x().whileTrue(m_elevator.sysIdQuasistatic(Direction.kForward)); testJoystick.y().whileTrue(m_elevator.sysIdQuasistatic(Direction.kReverse)); - //run sysID routines for arm - testJoystick.a().whileTrue(m_arm.sysIdDynamic(Direction.kForward)); - testJoystick.b().whileTrue(m_arm.sysIdDynamic(Direction.kReverse)); - testJoystick.x().whileTrue(m_arm.sysIdQuasistatic(Direction.kForward)); - testJoystick.y().whileTrue(m_arm.sysIdQuasistatic(Direction.kReverse)); - if (Utils.isSimulation()) { drivetrain.resetPose(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); } diff --git a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java index 7fc7f0dd..5b56998b 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java @@ -1,17 +1,36 @@ package frc.robot.subsystems.lifter; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.swerve.SwerveRequest; + import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutDistance; +import edu.wpi.first.units.measure.MutLinearVelocity; +import edu.wpi.first.units.measure.MutVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; - +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.ElevatorConstants; + + public class ElevatorSubsystem extends SubsystemBase{ private final ElevatorIOHardware m_IO = new ElevatorIOHardware(); private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward( @@ -99,4 +118,38 @@ public void periodic() { SmartDashboard.putNumber("elevator/position", m_IO.getPosition()); SmartDashboard.putNumber("elevator/velocity", m_IO.getVelocity()); } + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage m_appliedVoltage = Volts.mutable(0); + // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. + private final MutDistance m_distance = Meters.mutable(0); + // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. + private final MutLinearVelocity m_velocity = MetersPerSecond.mutable(0); + + + private final SysIdRoutine m_sysIDRoutineElevatorRoutine = new SysIdRoutine( + new SysIdRoutine.Config(Volts.per(Seconds).of(0.5), Volts.of(3), Seconds.of(10)), + new SysIdRoutine.Mechanism( + output -> m_IO.setVoltage(output.in(Volts)), + log -> { + // Record a frame for the shooter motor. + log.motor("elevatorsysID") + .voltage( + m_appliedVoltage.mut_replace( + m_IO.getVoltage(), Volts)) + .linearPosition(m_distance.mut_replace(m_IO.getPosition(), Meters)) + .linearVelocity( + m_velocity.mut_replace(m_IO.getVelocity(), MetersPerSecond)); + }, + this + ) + ); + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIDRoutineElevatorRoutine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIDRoutineElevatorRoutine.dynamic(direction); + } }