Skip to content

Commit

Permalink
Merge branch 'SimpleArmSupport' of https://github.com/DevilBotz2876/R…
Browse files Browse the repository at this point in the history
…omiTutorial2023 into SimpleArmSupport
  • Loading branch information
BrownGenius committed Nov 30, 2023
2 parents 66fdec9 + 1ad3b7c commit efb4b82
Show file tree
Hide file tree
Showing 3 changed files with 19 additions and 9 deletions.
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -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
}
Expand Down
17 changes: 11 additions & 6 deletions src/main/java/frc/robot/commands/ArmCommand.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,12 +6,16 @@
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
Expand All @@ -37,6 +41,7 @@ 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 newBasePositionDelta = MathUtil.applyDeadband(m_baseSpeed.getAsDouble(), m_deadband);
// Scale the speed as desired to reduce sensitivity
Expand Down
6 changes: 5 additions & 1 deletion src/main/java/frc/robot/subsystems/Arm.java
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,11 @@ public class Arm extends SubsystemBase {
// private final Servo m_armElbowServo = new Servo(2);

/**
* Moves the arm base to the desired position. 0.0 is max down and 1.0 is max up
* 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]
*/
Expand Down

0 comments on commit efb4b82

Please sign in to comment.