From e32b4c82ad4c1a6d2415d1295ddc25a907bd7a6e Mon Sep 17 00:00:00 2001 From: Nilesh Agarwalla Date: Wed, 29 Nov 2023 10:54:28 -0500 Subject: [PATCH 1/4] Added 1-axis arm support --- src/main/java/frc/robot/RobotContainer.java | 13 +++++ .../java/frc/robot/commands/ArmCommand.java | 54 +++++++++++++++++++ src/main/java/frc/robot/subsystems/Arm.java | 36 +++++++++++++ 3 files changed, 103 insertions(+) create mode 100644 src/main/java/frc/robot/commands/ArmCommand.java create mode 100644 src/main/java/frc/robot/subsystems/Arm.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f7f02ee..bc525c5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,9 +17,11 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.SysId; import frc.robot.commands.ArcadeDrive; +import frc.robot.commands.ArmCommand; import frc.robot.commands.AutonomousDistance; import frc.robot.commands.AutonomousTime; import frc.robot.commands.DriveVision; +import frc.robot.subsystems.Arm; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.OnBoardIO; import frc.robot.subsystems.OnBoardIO.ChannelMode; @@ -35,6 +37,10 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final Drivetrain m_drivetrain = new Drivetrain(new DriveIORomi()); private final OnBoardIO m_onboardIO = new OnBoardIO(ChannelMode.INPUT, ChannelMode.INPUT); + // START: Setup arm + // Create an arm sub-system + private final Arm m_arm = new Arm(); + // END: Setup arm // Assumes a XBox controller plugged into channnel 0 private final XboxController m_controller = new XboxController(0); @@ -96,6 +102,13 @@ private void configureButtonBindings() { Trigger xButton = new JoystickButton(m_controller, XboxController.Button.kX.value); xButton.whileTrue(new DriveVision(m_drivetrain)); // END: Setup photonvision + + // START: Setup arm + // Use the controller's right stick's forward/back (Y-axis) to control the arm base speed + // In this case, we want "forward" = "arm up", but forward is reported as a negative value from + // the stick, so we negate the returned value. + m_arm.setDefaultCommand(new ArmCommand(m_arm, () -> -m_controller.getRightY())); + // END: Setup arm } /** diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java new file mode 100644 index 0000000..980e113 --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -0,0 +1,54 @@ +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Arm; +import java.util.function.DoubleSupplier; + +public class ArmCommand extends CommandBase { + private final Arm m_arm; // the arm subsystem used by this comment + private final DoubleSupplier m_baseSpeed; // the supplier of the desired speed + private final double m_defaultPosition = + 0.5; // default start position for the arm when the command is first scheduled + private final double m_deadband = + 0.05; // to prevent stick drift, this value sets the min absolute value the speed needs to be + // before we assume it is not zero + private final double m_speedScale = + 16; // to reduce stick sensitivity, this value indicates how much to scale the returned speed + // by + private final double m_minBaseRange = -1; // min range for the arm's base + private final double m_maxBaseRange = 1; // max range for the arm's base + + public ArmCommand(Arm arm, DoubleSupplier baseSpeed) { + m_arm = arm; + m_baseSpeed = baseSpeed; + + // To prevent scheduling conflicts, all commands need to indicate which sub-system(s) it uses + addRequirements(arm); + } + + @Override + public void initialize() { + // We set the arm's base position to the default position when this command is first scheduled + m_arm.setArmBasePosition(m_defaultPosition); + } + + @Override + public void execute() { + // Variable that will store the new calculated arm position + double newArmPosition; + // Determine the requested speed, but ignoring inputs near-zero (i.e. +/= m_deadband) + double currentShoulderSpeed = MathUtil.applyDeadband(m_baseSpeed.getAsDouble(), m_deadband); + // Scale the speed as desired to reduce sensitivity + currentShoulderSpeed /= m_speedScale; + + // Calculate the arm position by adding the computer speed to the arm base's current position + newArmPosition = m_arm.getArmBasePosition() + currentShoulderSpeed; + + // Clamp the arm's upper/lower position to the min/max range allowed + newArmPosition = MathUtil.clamp(newArmPosition, m_minBaseRange, m_maxBaseRange); + + // Finally, set the arm base positon to the new calculated arm position. + m_arm.setArmBasePosition(newArmPosition); + } +} diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java new file mode 100644 index 0000000..21a023f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -0,0 +1,36 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Arm extends SubsystemBase { + // This code assumes we are using the ROMI's external IO configuration and the arm base servo is + // connected to Romi Pin EXT4 which is configured as PWM and mapped to port 3 + // https://docs.wpilib.org/en/stable/docs/romi-robot/web-ui.html#external-io-configuration + private final Servo m_armBaseServo = new Servo(3); + + // Note: If you have a multi-axis arm, you could create another Servo object using a differemt PWM + // channel and add the corresponding methods for controlling the position. + // private final Servo m_armElbowServo = new Servo(2); + + /** + * Moves the arm base to the desired position. -1.0 is max down, and 1.0 is max up + * + * @param position desired target position for arm base [-1.0 to 1.0] + */ + public void setArmBasePosition(double position) { + // Note: This code doesn't validate the requested position. If we don't want the arm to move + // within the entire range (e.g. due to physical constraints), we could clamp the position to + // the min/max values before setting the servo's position + m_armBaseServo.set(position); + } + + /** + * @return the current desired target position for arm base. Note: since the basic servo doesn't + * have any feedback, this is just the _requested_ position. The actual servo position is not + * known. + */ + public double getArmBasePosition() { + return m_armBaseServo.get(); + } +} From 1ad3b7c18a73bc0491f29ab27de0023e6e1daf5c Mon Sep 17 00:00:00 2001 From: Nilesh Agarwalla Date: Wed, 29 Nov 2023 11:05:32 -0500 Subject: [PATCH 2/4] Updated comments --- src/main/java/frc/robot/RobotContainer.java | 5 +-- .../java/frc/robot/commands/ArmCommand.java | 34 +++++++++++++------ src/main/java/frc/robot/subsystems/Arm.java | 4 +++ 3 files changed, 30 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bc525c5..87d582f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -105,8 +105,9 @@ private void configureButtonBindings() { // START: Setup arm // Use the controller's right stick's forward/back (Y-axis) to control the arm base speed - // In this case, we want "forward" = "arm up", but forward is reported as a negative value from - // the stick, so we negate the returned value. + // In this case, we want "forward" = "arm up" = positive value, but forward is reported as a + // negative value from + // the controller's stick, so we negate the returned value. m_arm.setDefaultCommand(new ArmCommand(m_arm, () -> -m_controller.getRightY())); // END: Setup arm } diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index 980e113..efe7a16 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -6,18 +6,28 @@ import java.util.function.DoubleSupplier; public class ArmCommand extends CommandBase { - private final Arm m_arm; // the arm subsystem used by this comment - private final DoubleSupplier m_baseSpeed; // the supplier of the desired speed - private final double m_defaultPosition = - 0.5; // default start position for the arm when the command is first scheduled - private final double m_deadband = - 0.05; // to prevent stick drift, this value sets the min absolute value the speed needs to be + // the arm subsystem used by this comment + private final Arm m_arm; + + // the supplier of the desired speed + private final DoubleSupplier m_baseSpeed; + + // default start position for the arm when the command is first scheduled + private final double m_defaultPosition = 0.5; + + // to prevent stick drift, this value sets the min absolute value the speed needs to be // before we assume it is not zero - private final double m_speedScale = - 16; // to reduce stick sensitivity, this value indicates how much to scale the returned speed - // by - private final double m_minBaseRange = -1; // min range for the arm's base - private final double m_maxBaseRange = 1; // max range for the arm's base + private final double m_deadband = 0.05; + + // to reduce stick sensitivity, this value indicates how much to scale the requested + // speed. + private final double m_speedScale = 16; + + // min range for the arm's base + private final double m_minBaseRange = -1; + + // max range for the arm's base + private final double m_maxBaseRange = 1; public ArmCommand(Arm arm, DoubleSupplier baseSpeed) { m_arm = arm; @@ -37,8 +47,10 @@ public void initialize() { public void execute() { // Variable that will store the new calculated arm position double newArmPosition; + // Determine the requested speed, but ignoring inputs near-zero (i.e. +/= m_deadband) double currentShoulderSpeed = MathUtil.applyDeadband(m_baseSpeed.getAsDouble(), m_deadband); + // Scale the speed as desired to reduce sensitivity currentShoulderSpeed /= m_speedScale; diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 21a023f..2140ba7 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -16,6 +16,10 @@ public class Arm extends SubsystemBase { /** * Moves the arm base to the desired position. -1.0 is max down, and 1.0 is max up * + *

Note: The concept of "up and down" depends on how the servo is mounted to the arm. E.g. if + * the servo is flipped, the directions would be reversed. This module abstracts the hardware + * implementation details and follows the convention of 1.0 is up. + * * @param position desired target position for arm base [-1.0 to 1.0] */ public void setArmBasePosition(double position) { From 66fdec9a2664591f2e4070a14252ad1fa539730f Mon Sep 17 00:00:00 2001 From: Nilesh Agarwalla Date: Wed, 29 Nov 2023 19:49:55 -0500 Subject: [PATCH 3/4] Changes based on code review feedback --- src/main/java/frc/robot/commands/ArmCommand.java | 8 ++++---- src/main/java/frc/robot/subsystems/Arm.java | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index 980e113..375b79e 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -16,7 +16,7 @@ public class ArmCommand extends CommandBase { private final double m_speedScale = 16; // to reduce stick sensitivity, this value indicates how much to scale the returned speed // by - private final double m_minBaseRange = -1; // min range for the arm's base + private final double m_minBaseRange = 0; // min range for the arm's base private final double m_maxBaseRange = 1; // max range for the arm's base public ArmCommand(Arm arm, DoubleSupplier baseSpeed) { @@ -38,12 +38,12 @@ public void execute() { // Variable that will store the new calculated arm position double newArmPosition; // Determine the requested speed, but ignoring inputs near-zero (i.e. +/= m_deadband) - double currentShoulderSpeed = MathUtil.applyDeadband(m_baseSpeed.getAsDouble(), m_deadband); + double newBasePositionDelta = MathUtil.applyDeadband(m_baseSpeed.getAsDouble(), m_deadband); // Scale the speed as desired to reduce sensitivity - currentShoulderSpeed /= m_speedScale; + newBasePositionDelta /= m_speedScale; // Calculate the arm position by adding the computer speed to the arm base's current position - newArmPosition = m_arm.getArmBasePosition() + currentShoulderSpeed; + newArmPosition = m_arm.getArmBasePosition() + newBasePositionDelta; // Clamp the arm's upper/lower position to the min/max range allowed newArmPosition = MathUtil.clamp(newArmPosition, m_minBaseRange, m_maxBaseRange); diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 21a023f..e280c4d 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -14,9 +14,9 @@ public class Arm extends SubsystemBase { // private final Servo m_armElbowServo = new Servo(2); /** - * Moves the arm base to the desired position. -1.0 is max down, and 1.0 is max up + * Moves the arm base to the desired position. 0.0 is max down and 1.0 is max up * - * @param position desired target position for arm base [-1.0 to 1.0] + * @param position desired target position for arm base [0.0 to 1.0] */ public void setArmBasePosition(double position) { // Note: This code doesn't validate the requested position. If we don't want the arm to move From c602bbd6df9d6fa640025d1877a9914e7690da49 Mon Sep 17 00:00:00 2001 From: Nilesh Agarwalla Date: Wed, 29 Nov 2023 19:57:53 -0500 Subject: [PATCH 4/4] Fix Compilation Error --- src/main/java/frc/robot/commands/ArmCommand.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index 9d64e27..8e55b4c 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -15,6 +15,9 @@ public class ArmCommand extends CommandBase { // default start position for the arm when the command is first scheduled private final double m_defaultPosition = 0.5; + private final double m_deadband = + 0.05; // to prevent stick drift, this value sets the min absolute value the speed needs to be + // to prevent stick drift, this value sets the min absolute value the speed needs to be // before we assume it is not zero private final double m_speedScale =