diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 87d582f..7836da4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,8 +21,10 @@ import frc.robot.commands.AutonomousDistance; import frc.robot.commands.AutonomousTime; import frc.robot.commands.DriveVision; +import frc.robot.commands.GripperCommand; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Gripper; import frc.robot.subsystems.OnBoardIO; import frc.robot.subsystems.OnBoardIO.ChannelMode; import frc.robot.subsystems.drive.DriveIORomi; @@ -41,6 +43,7 @@ public class RobotContainer { // Create an arm sub-system private final Arm m_arm = new Arm(); // END: Setup arm + private final Gripper m_gripper = new Gripper(); // Assumes a XBox controller plugged into channnel 0 private final XboxController m_controller = new XboxController(0); @@ -108,8 +111,9 @@ private void configureButtonBindings() { // 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())); + m_arm.setDefaultCommand(new ArmCommand(m_arm, () -> m_controller.getRightX())); // END: Setup arm + m_gripper.setDefaultCommand(new GripperCommand(m_gripper, () -> m_controller.getRightY())); } /** diff --git a/src/main/java/frc/robot/commands/GripperCommand.java b/src/main/java/frc/robot/commands/GripperCommand.java new file mode 100644 index 0000000..be49167 --- /dev/null +++ b/src/main/java/frc/robot/commands/GripperCommand.java @@ -0,0 +1,64 @@ +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Gripper; +import java.util.function.DoubleSupplier; + +public class GripperCommand extends CommandBase { + // the gripper subsystem used by this comment + private final Gripper m_gripper; + + // the supplier of the desired speed + private final DoubleSupplier m_baseSpeed; + + // default start position for the gripper 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 gripper's base + private final double m_maxBaseRange = 1; // max range for the gripper's base + + public GripperCommand(Gripper gripper, DoubleSupplier baseSpeed) { + m_gripper = gripper; + m_baseSpeed = baseSpeed; + + // To prevent scheduling conflicts, all commands need to indicate which sub-system(s) it uses + addRequirements(gripper); + } + + @Override + public void initialize() { + // We set the gripper's base position to the default position when this command is first + // scheduled + m_gripper.setClawPosition(m_defaultPosition); + } + + @Override + public void execute() { + // Variable that will store the new calculated gripper position + double newClawPosition; + + // 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 gripper position by adding the computer speed to the gripper base's current + // position + newClawPosition = m_gripper.getClawPosition() + newBasePositionDelta; + + // Clamp the gripper's upper/lower position to the min/max range allowed + newClawPosition = MathUtil.clamp(newClawPosition, m_minBaseRange, m_maxBaseRange); + + // Finally, set the gripper base positon to the new calculated gripper position. + m_gripper.setClawPosition(newClawPosition); + } +} diff --git a/src/main/java/frc/robot/subsystems/Gripper.java b/src/main/java/frc/robot/subsystems/Gripper.java new file mode 100644 index 0000000..8742472 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Gripper.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Gripper extends SubsystemBase { + // This code assumes we are using the ROMI's external IO configuration and the arm claw servo is + // connected to Romi Pin EXT4 which is configured as PWM and mapped to port 2 + // https://docs.wpilib.org/en/stable/docs/romi-robot/web-ui.html#external-io-configuration + private final Servo m_armClawServo = new Servo(2); + + // 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 claw 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 claw [0.0 to 1.0] + */ + public void setClawPosition(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_armClawServo.set(position); + } + + /** + * @return the current desired target position for arm claw. 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 getClawPosition() { + return m_armClawServo.get(); + } +}