From 19f34cff99b8d3f78b1daa59d212c5592811724a Mon Sep 17 00:00:00 2001 From: kmshah2 Date: Fri, 6 Jun 2025 13:20:58 -0500 Subject: [PATCH 1/3] created arm subsystem with IO and TalonFX classes --- src/main/java/frc/robot/RobotContainer.java | 18 +++++++---- src/main/java/frc/robot/subsystems/Arm.java | 32 ------------------- .../java/frc/robot/subsystems/arm/Arm.java | 29 +++++++++++++++++ .../java/frc/robot/subsystems/arm/ArmIO.java | 15 +++++++++ .../frc/robot/subsystems/arm/ArmIOMotor.java | 24 ++++++++++++++ 5 files changed, 80 insertions(+), 38 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/Arm.java create mode 100644 src/main/java/frc/robot/subsystems/arm/Arm.java create mode 100644 src/main/java/frc/robot/subsystems/arm/ArmIO.java create mode 100644 src/main/java/frc/robot/subsystems/arm/ArmIOMotor.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4eacdc4..5583fc0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,12 +10,15 @@ import frc.robot.commands.ArcadeDrive; import frc.robot.commands.AutonomousDistance; import frc.robot.commands.AutonomousTime; -import frc.robot.subsystems.Arm; import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.arm.ArmIO; +import frc.robot.subsystems.arm.ArmIOMotor; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.xrp.XRPOnBoardIO; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -31,7 +34,7 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final Drivetrain m_drivetrain = new Drivetrain(); private final XRPOnBoardIO m_onboardIO = new XRPOnBoardIO(); - private final Arm m_arm = new Arm(); + private Arm m_arm = new Arm(new ArmIOMotor()); // Assumes a gamepad plugged into channel 0 private final Joystick m_controller = new Joystick(0); @@ -64,13 +67,16 @@ private void configureButtonBindings() { JoystickButton joystickAButton = new JoystickButton(m_controller, 1); joystickAButton - .onTrue(new InstantCommand(() -> m_arm.setAngle(45.0), m_arm)) - .onFalse(new InstantCommand(() -> m_arm.setAngle(0.0), m_arm)); + .onTrue(new InstantCommand(() -> m_arm.goToAngle(45.0), m_arm)) + .onFalse(new InstantCommand(() -> m_arm.zeroPosition(), m_arm)); JoystickButton joystickBButton = new JoystickButton(m_controller, 2); joystickBButton - .onTrue(new InstantCommand(() -> m_arm.setAngle(90.0), m_arm)) - .onFalse(new InstantCommand(() -> m_arm.setAngle(0.0), m_arm)); + .onTrue(Commands.sequence( + new InstantCommand(() -> m_arm.goToAngle(90.0), m_arm), + new InstantCommand(() -> m_arm.getAngle(), m_arm), + new InstantCommand(() -> m_arm.goToAngle(180), m_arm))) + .onFalse(new InstantCommand(() -> m_arm.zeroPosition(), m_arm)); // Setup SmartDashboard options m_chooser.setDefaultOption("Auto Routine Distance", new AutonomousDistance(m_drivetrain)); diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java deleted file mode 100644 index 530a999..0000000 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj.xrp.XRPServo; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Arm extends SubsystemBase { - private final XRPServo m_armServo; - - /** Creates a new Arm. */ - public Arm() { - // Device number 4 maps to the physical Servo 1 port on the XRP - m_armServo = new XRPServo(4); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - /** - * Set the current angle of the arm (0 - 180 degrees). - * - * @param angleDeg Desired arm angle in degrees - */ - public void setAngle(double angleDeg) { - m_armServo.setAngle(angleDeg); - } -} diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java new file mode 100644 index 0000000..aece926 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -0,0 +1,29 @@ +package frc.robot.subsystems.arm; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Arm extends SubsystemBase { + private final ArmIO io; + + public Arm(ArmIO io) { + this.io = io; + zeroPosition(); + } + + @Override + public void periodic() { + // This will be called every 20 ms but we don't have anything specific to do here yet (update inputs) + } + + public void goToAngle(double angleDeg) { + io.setAngle(angleDeg); + } + + public double getAngle() { + return io.getAngle(); + } + + public void zeroPosition() { + io.zeroPosition(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java new file mode 100644 index 0000000..b2cff1b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems.arm; + +import edu.wpi.first.wpilibj.xrp.XRPServo; + +public interface ArmIO { + + public default void setAngle(double angleDeg){} + + public default double getAngle(){ + return 0; + } + + public default void zeroPosition() {} +} + diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOMotor.java b/src/main/java/frc/robot/subsystems/arm/ArmIOMotor.java new file mode 100644 index 0000000..6d51fb1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOMotor.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.arm; + +import edu.wpi.first.wpilibj.xrp.XRPServo; + +public class ArmIOMotor implements ArmIO{ + + private final XRPServo armMotor = new XRPServo(4); + + @Override + public void setAngle(double angleDeg) { + armMotor.setAngle(angleDeg); + } + + @Override + public double getAngle() { + return armMotor.getAngle(); + } + + @Override + public void zeroPosition() { + armMotor.setAngle(0); + } + +} From 66aff2a74ace60a07736b89faf85c3d404a1e882 Mon Sep 17 00:00:00 2001 From: kmshah2 Date: Tue, 10 Jun 2025 22:53:21 -0500 Subject: [PATCH 2/3] Fixed changes, tested and works. --- src/main/java/frc/robot/RobotContainer.java | 15 +++++++-------- src/main/java/frc/robot/subsystems/arm/Arm.java | 14 ++++++-------- src/main/java/frc/robot/subsystems/arm/ArmIO.java | 12 +++++++----- .../arm/{ArmIOMotor.java => ArmIOXRP.java} | 15 +++++---------- 4 files changed, 25 insertions(+), 31 deletions(-) rename src/main/java/frc/robot/subsystems/arm/{ArmIOMotor.java => ArmIOXRP.java} (57%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5583fc0..c800364 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,7 +13,7 @@ import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.ArmIO; -import frc.robot.subsystems.arm.ArmIOMotor; +import frc.robot.subsystems.arm.ArmIOXRP; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.xrp.XRPOnBoardIO; @@ -34,7 +34,7 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final Drivetrain m_drivetrain = new Drivetrain(); private final XRPOnBoardIO m_onboardIO = new XRPOnBoardIO(); - private Arm m_arm = new Arm(new ArmIOMotor()); + private Arm m_arm = new Arm(new ArmIOXRP()); // Assumes a gamepad plugged into channel 0 private final Joystick m_controller = new Joystick(0); @@ -68,15 +68,14 @@ private void configureButtonBindings() { JoystickButton joystickAButton = new JoystickButton(m_controller, 1); joystickAButton .onTrue(new InstantCommand(() -> m_arm.goToAngle(45.0), m_arm)) - .onFalse(new InstantCommand(() -> m_arm.zeroPosition(), m_arm)); + .onFalse(new InstantCommand(() -> m_arm.goToAngle(0.0), m_arm)); + JoystickButton joystickBButton = new JoystickButton(m_controller, 2); joystickBButton - .onTrue(Commands.sequence( - new InstantCommand(() -> m_arm.goToAngle(90.0), m_arm), - new InstantCommand(() -> m_arm.getAngle(), m_arm), - new InstantCommand(() -> m_arm.goToAngle(180), m_arm))) - .onFalse(new InstantCommand(() -> m_arm.zeroPosition(), m_arm)); + .onTrue( + new InstantCommand(() -> m_arm.goToAngle(90.0), m_arm)) + .onFalse(new InstantCommand(() -> m_arm.goToAngle(0.0), m_arm)); // Setup SmartDashboard options m_chooser.setDefaultOption("Auto Routine Distance", new AutonomousDistance(m_drivetrain)); diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index aece926..eeac592 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -1,18 +1,20 @@ package frc.robot.subsystems.arm; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.arm.ArmIO.ArmIOInputs; public class Arm extends SubsystemBase { - private final ArmIO io; + private ArmIO io; + + private final ArmIOInputs inputs = new ArmIOInputs(); public Arm(ArmIO io) { this.io = io; - zeroPosition(); } @Override public void periodic() { - // This will be called every 20 ms but we don't have anything specific to do here yet (update inputs) + io.updateInputs(inputs); } public void goToAngle(double angleDeg) { @@ -20,10 +22,6 @@ public void goToAngle(double angleDeg) { } public double getAngle() { - return io.getAngle(); - } - - public void zeroPosition() { - io.zeroPosition(); + return inputs.positionDeg; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index b2cff1b..8151e02 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -4,12 +4,14 @@ public interface ArmIO { - public default void setAngle(double angleDeg){} + public static class ArmIOInputs { + + public double positionDeg = 0.0; + + } - public default double getAngle(){ - return 0; - } + public default void updateInputs(ArmIOInputs inputs){} - public default void zeroPosition() {} + public default void setAngle(double angleDeg){} } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOMotor.java b/src/main/java/frc/robot/subsystems/arm/ArmIOXRP.java similarity index 57% rename from src/main/java/frc/robot/subsystems/arm/ArmIOMotor.java rename to src/main/java/frc/robot/subsystems/arm/ArmIOXRP.java index 6d51fb1..a6adf43 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOMotor.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOXRP.java @@ -2,23 +2,18 @@ import edu.wpi.first.wpilibj.xrp.XRPServo; -public class ArmIOMotor implements ArmIO{ +public class ArmIOXRP implements ArmIO{ private final XRPServo armMotor = new XRPServo(4); @Override - public void setAngle(double angleDeg) { - armMotor.setAngle(angleDeg); + public void updateInputs(ArmIOInputs inputs) { + inputs.positionDeg = armMotor.getAngle(); } @Override - public double getAngle() { - return armMotor.getAngle(); - } - - @Override - public void zeroPosition() { - armMotor.setAngle(0); + public void setAngle(double angleDeg) { + armMotor.setAngle(angleDeg); } } From 27d80a5b46f3a9a9095f87d05818d1f4067ee1b1 Mon Sep 17 00:00:00 2001 From: kmshah2 Date: Tue, 10 Jun 2025 23:17:29 -0500 Subject: [PATCH 3/3] Created a command for practice --- src/main/java/frc/robot/RobotContainer.java | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c800364..ef0fa1e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,7 @@ import frc.robot.commands.ArcadeDrive; import frc.robot.commands.AutonomousDistance; import frc.robot.commands.AutonomousTime; +import frc.robot.commands.DriveDistance; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.ArmIO; @@ -77,6 +78,19 @@ private void configureButtonBindings() { new InstantCommand(() -> m_arm.goToAngle(90.0), m_arm)) .onFalse(new InstantCommand(() -> m_arm.goToAngle(0.0), m_arm)); + + JoystickButton joystickCButton = new JoystickButton(m_controller, 3); + + joystickCButton + .onTrue(Commands.sequence( + new DriveDistance(0.5, 10, m_drivetrain), + new InstantCommand(() -> m_arm.goToAngle(0.0), m_arm), + Commands.waitSeconds(1), + new InstantCommand(() -> m_arm.goToAngle(180.0), m_arm), + new DriveDistance(-0.5, 10, m_drivetrain) + )) + .onFalse(new InstantCommand(() -> m_arm.goToAngle(0.0), m_arm)); + // Setup SmartDashboard options m_chooser.setDefaultOption("Auto Routine Distance", new AutonomousDistance(m_drivetrain)); m_chooser.addOption("Auto Routine Time", new AutonomousTime(m_drivetrain));