Skip to content

Commit

Permalink
Updated comments
Browse files Browse the repository at this point in the history
  • Loading branch information
BrownGenius committed Nov 29, 2023
1 parent e32b4c8 commit 1ad3b7c
Show file tree
Hide file tree
Showing 3 changed files with 30 additions and 13 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
34 changes: 23 additions & 11 deletions src/main/java/frc/robot/commands/ArmCommand.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;

Expand Down
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/subsystems/Arm.java
Original file line number Diff line number Diff line change
Expand Up @@ -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
*
* <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 [-1.0 to 1.0]
*/
public void setArmBasePosition(double position) {
Expand Down

0 comments on commit 1ad3b7c

Please sign in to comment.