From 767b49407e3bc0ff534836fd7ad2c29c7f58db44 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sun, 7 Jan 2024 18:23:20 -0500 Subject: [PATCH] create the commands for the arm & claw --- src/main/java/frc/robot/RobotContainer.java | 97 +++++++++++++++++-- .../java/frc/robot/subsystems/arm/Arm.java | 23 +++++ .../java/frc/robot/subsystems/arm/ArmIO.java | 4 +- .../robot/subsystems/arm/ArmIOSparkMax.java | 2 +- .../java/frc/robot/subsystems/claw/Claw.java | 24 +++++ .../frc/robot/subsystems/claw/ClawIO.java | 4 +- .../robot/subsystems/claw/ClawIOSparkMax.java | 2 +- 7 files changed, 141 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e74bdd4..2de3c5c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,14 +5,22 @@ package frc.robot; import com.dacubeking.AutoBuilder.robot.GuiAuto; +import com.dacubeking.AutoBuilder.robot.annotations.AutoBuilderAccessible; import com.dacubeking.AutoBuilder.robot.robotinterface.AutonomousContainer; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.RamseteController; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.arm.ArmIO; +import frc.robot.subsystems.arm.ArmIOSparkMax; +import frc.robot.subsystems.claw.Claw; +import frc.robot.subsystems.claw.ClawIO; +import frc.robot.subsystems.claw.ClawIOSparkMax; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveIO; import frc.robot.subsystems.drive.DriveIOFalcon500; @@ -28,7 +36,9 @@ */ public class RobotContainer { // Subsystems - private final Drive drive; + private Drive drive; + private Claw claw; + private Arm arm; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -36,7 +46,11 @@ public class RobotContainer { private final LoggedDashboardChooser autoChooser = new LoggedDashboardChooser<>("Auto Choices"); private final LoggedDashboardChooser sideChooser = new LoggedDashboardChooser<>("Side Chooser"); - private final LoggedDashboardNumber flywheelSpeedInput = new LoggedDashboardNumber("Flywheel Speed", 1500.0); + private final LoggedDashboardNumber flywheelSpeed = new LoggedDashboardNumber("Flywheel Speed", 1500.0); + private final LoggedDashboardNumber shootAngle = new LoggedDashboardNumber("Shoot Angle", 40); + private final LoggedDashboardNumber pickupAngle = new LoggedDashboardNumber("Pickup Angle", -20); + private final LoggedDashboardNumber trapAngle = new LoggedDashboardNumber("Trap Angle", 100); + /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -44,17 +58,46 @@ public class RobotContainer { public RobotContainer() { switch (Constants.currentMode) { // Real robot, instantiate hardware IO implementations - case REAL -> drive = new Drive(new DriveIOFalcon500()); + case REAL -> { + drive = new Drive(new DriveIOFalcon500()); + claw = new Claw(new ClawIOSparkMax()); + arm = new Arm(new ArmIOSparkMax()); + } // drive = new Drive(new DriveIOFalcon500()); // Sim robot, instantiate physics sim IO implementations - case SIM -> drive = new Drive(new DriveIOSim()); + case SIM -> { + drive = new Drive(new DriveIOSim()); + claw = new Claw(new ClawIOSparkMax()); + arm = new Arm(new ArmIOSparkMax()); + } // Replayed robot, disable IO implementations - default -> drive = new Drive(new DriveIO() { - }); + default -> { + drive = new Drive(new DriveIO() { + }); + claw = new Claw(new ClawIO() { + }); + arm = new Arm(new ArmIO() { + }); + } } + // Configure the button bindings + configureButtonBindings(); + + claw.setDefaultCommand(new RunCommand( + () -> { + claw.setShooterVelocity(0, 0); + claw.setIntakeVelocity(0); + }, claw + )); + + arm.setDefaultCommand(new RunCommand( + () -> arm.setArmPosition(arm.getArmPosition()), + arm + )); + // Initialize autonomous container AutonomousContainer.getInstance().setDebugPrints(true); AutonomousContainer.getInstance().initialize( @@ -81,11 +124,47 @@ public RobotContainer() { sideChooser.addOption("Red", "red"); AutonomousContainer.getInstance().getAutonomousNames().forEach(name -> autoChooser.addOption(name, name)); - - // Configure the button bindings - configureButtonBindings(); } + + @AutoBuilderAccessible + Command intakeCommand = new RunCommand( + () -> { + claw.setIntakeVelocity(0.5); + arm.setArmPosition(pickupAngle.get()); + }, claw, arm + ); + + @AutoBuilderAccessible + Command outtakeCommand = new RunCommand( + () -> { + claw.setIntakeVelocity(-0.5); + }, claw + ); + + @AutoBuilderAccessible + Command shootCommand = new RunCommand( + () -> { + claw.setShooterVelocity(flywheelSpeed.get(), flywheelSpeed.get()); + arm.setArmPosition(shootAngle.get()); + }, claw, arm) + .until(() -> Math.abs(arm.getArmPosition() - shootAngle.get()) < 5 + && Math.abs(claw.getShooterVelocity() - flywheelSpeed.get()) < 100) + .andThen(() -> { + claw.setIntakeVelocity(1000); + claw.setShooterVelocity(flywheelSpeed.get(), flywheelSpeed.get()); + arm.setArmPosition(shootAngle.get()); + }, claw, arm); + + @AutoBuilderAccessible + Command scoreTrapCommand = new RunCommand(() -> { + arm.setArmPosition(trapAngle.get()); + claw.setIntakeVelocity(0); + }, arm, claw) + .until(() -> Math.abs(arm.getArmPosition() - trapAngle.get()) < 5) + .andThen(() -> claw.setIntakeVelocity(100), arm, claw); + + /** * Use this method to define your button->command mappings. Buttons can be created by instantiating a * {@link GenericHID} or one of its subclasses ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index da29d53..a783fc1 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -1,7 +1,30 @@ package frc.robot.subsystems.arm; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.arm.ArmIO.ArmInputs; public class Arm extends SubsystemBase { + ArmIO io; + ArmInputs inputs = new ArmInputs(); + + public Arm(ArmIO io) { + this.io = io; + } + + /** + * @param position The position to set the arm to in degrees + */ + public void setArmPosition(double position) { + io.setArmPosition(position); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + } + + public double getArmPosition() { + return inputs.armPosition; + } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index cb8b1eb..ef9e393 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.arm; public interface ArmIO { - public static class ArmIOInputs { + public static class ArmInputs { public double armPosition = 0.0; public double armVelocity = 0.0; public double armCurrent = 0.0; @@ -12,7 +12,7 @@ public static class ArmIOInputs { /** * Updates the set of loggable inputs. */ - public default void updateInputs(ArmIOInputs inputs) { + public default void updateInputs(ArmInputs inputs) { } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java index 920acb8..d0af485 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java @@ -22,7 +22,7 @@ public ArmIOSparkMax() { } - public void updateInputs(ArmIOInputs inputs) { + public void updateInputs(ArmInputs inputs) { inputs.armPosition = arm.getEncoder().getPosition(); inputs.armVelocity = arm.getEncoder().getVelocity(); inputs.armCurrent = arm.getOutputCurrent(); diff --git a/src/main/java/frc/robot/subsystems/claw/Claw.java b/src/main/java/frc/robot/subsystems/claw/Claw.java index 421a8e7..bc813aa 100644 --- a/src/main/java/frc/robot/subsystems/claw/Claw.java +++ b/src/main/java/frc/robot/subsystems/claw/Claw.java @@ -1,7 +1,31 @@ package frc.robot.subsystems.claw; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.claw.ClawIO.ClawInputs; public class Claw extends SubsystemBase { + ClawIO io; + ClawInputs inputs = new ClawInputs(); + + public Claw(ClawIO io) { + this.io = io; + } + + public void setShooterVelocity(double leftVelocity, double rightVelocity) { + io.setShooterVelocity(leftVelocity, rightVelocity); + } + + public void setIntakeVelocity(double velocity) { + io.setIntakeVelocity(velocity); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + } + + public double getShooterVelocity() { + return (inputs.shooterLeftVelocity + inputs.shooterRightVelocity) / 2; + } } diff --git a/src/main/java/frc/robot/subsystems/claw/ClawIO.java b/src/main/java/frc/robot/subsystems/claw/ClawIO.java index 9148199..9a50505 100644 --- a/src/main/java/frc/robot/subsystems/claw/ClawIO.java +++ b/src/main/java/frc/robot/subsystems/claw/ClawIO.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.claw; public interface ClawIO { - public static class ClawIOInputs { + public static class ClawInputs { public double shooterLeftVoltage = 0.0; public double shooterRightVoltage = 0.0; public double shooterLeftCurrent = 0.0; @@ -26,7 +26,7 @@ public static class ClawIOInputs { /** * Updates the set of loggable inputs. */ - public default void updateInputs(ClawIOInputs inputs) { + public default void updateInputs(ClawInputs inputs) { } public default void setShooterVelocity(double leftVelocity, double rightVelocity) { diff --git a/src/main/java/frc/robot/subsystems/claw/ClawIOSparkMax.java b/src/main/java/frc/robot/subsystems/claw/ClawIOSparkMax.java index b995060..71e8382 100644 --- a/src/main/java/frc/robot/subsystems/claw/ClawIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/claw/ClawIOSparkMax.java @@ -49,7 +49,7 @@ public ClawIOSparkMax() { intake.getPIDController().setFF(0.0); } - public void updateInputs(ClawIOInputs inputs) { + public void updateInputs(ClawInputs inputs) { inputs.shooterLeftVoltage = shooterLeft.getBusVoltage(); inputs.shooterRightVoltage = shooterRight.getBusVoltage(); inputs.shooterLeftCurrent = shooterLeft.getOutputCurrent();