Merge branch 'main' of https://github.com/DevilBotz2876/RomiTutorial2023

 into team1111

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,14 @@ 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" = 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
   }
 
   /**

src/main/java/frc/robot/commands/ArmCommand.java

@@ -0,0 +1,62 @@
+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 {
+  // 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;
+
+  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 =
+      16; // to reduce stick sensitivity, this value indicates how much to scale the returned speed
+  // by
+  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) {
+    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 newBasePositionDelta = MathUtil.applyDeadband(m_baseSpeed.getAsDouble(), m_deadband);
+    // Scale the speed as desired to reduce sensitivity
+    newBasePositionDelta /= m_speedScale;
+
+    // Calculate the arm position by adding the computer speed to the arm base's current position
+    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);
+
+    // Finally, set the arm base positon to the new calculated arm position.
+    m_arm.setArmBasePosition(newArmPosition);
+  }
+}

src/main/java/frc/robot/subsystems/Arm.java

@@ -0,0 +1,40 @@
+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
+   *
+   * <p>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 [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
+    // 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();
+  }
+}