Merge pull request #6 from Ri3D-PSU/arm-&-claw

Climber, Arm, & Intake

src/main/java/frc/robot/RobotContainer.java

@@ -5,18 +5,30 @@
 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.StartEndCommand;
 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.climber.Climber;
+import frc.robot.subsystems.climber.ClimberIO;
+import frc.robot.subsystems.climber.ClimberIOSparkMax;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.DriveIO;
 import frc.robot.subsystems.drive.DriveIOFalcon500;
 import frc.robot.subsystems.drive.DriveIOSim;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.IntakeIO;
+import frc.robot.subsystems.intake.IntakeIOSparkMax;
 import org.jetbrains.annotations.Nullable;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
@@ -28,33 +40,76 @@
  */
 public class RobotContainer {
     // Subsystems
-    private final Drive drive;
+    private Drive drive;
+    private Intake intake;
+    private Arm arm;
+    private Climber climber;
     // Controller
     private final CommandXboxController controller = new CommandXboxController(0);
 
     // Dashboard inputs
     private final LoggedDashboardChooser<String> autoChooser = new LoggedDashboardChooser<>("Auto Choices");
     private final LoggedDashboardChooser<String> sideChooser = new LoggedDashboardChooser<>("Side Chooser");
 
-    private final LoggedDashboardNumber flywheelSpeedInput = new LoggedDashboardNumber("Flywheel Speed", 1500.0);
+    private final LoggedDashboardNumber shootSpeed = new LoggedDashboardNumber("Shoot Speed", 1500.0);
+    private final LoggedDashboardNumber shootAngle = new LoggedDashboardNumber("Shoot Angle", 40);
+    private final LoggedDashboardNumber pickupAngle = new LoggedDashboardNumber("Pickup Angle", -20);
+    private final LoggedDashboardNumber ampScoringAngle = new LoggedDashboardNumber("Amp Scoring Angle", 40);
+    private final LoggedDashboardNumber ampScoringSpeed = new LoggedDashboardNumber("Amp Scoring Speed", 1000.0);
+    private final LoggedDashboardNumber intakeSpeed = new LoggedDashboardNumber("Intake Speed", 1000);
+    private final LoggedDashboardNumber ejectSpeed = new LoggedDashboardNumber("Eject Speed", -1000);
+
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
     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());
+                intake = new Intake(new IntakeIOSparkMax());
+                arm = new Arm(new ArmIOSparkMax());
+                climber = new Climber(new ClimberIOSparkMax());
+            }
 
             // 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());
+                intake = new Intake(new IntakeIOSparkMax());
+                arm = new Arm(new ArmIOSparkMax());
+                climber = new Climber(new ClimberIOSparkMax());
+            }
 
             // Replayed robot, disable IO implementations
-            default -> drive = new Drive(new DriveIO() {
-            });
+            default -> {
+                drive = new Drive(new DriveIO() {
+                });
+                intake = new Intake(new IntakeIO() {
+                });
+                arm = new Arm(new ArmIO() {
+                });
+                climber = new Climber(new ClimberIO() {
+                });
+            }
         }
 
+        // Configure the button bindings
+        configureButtonBindings();
+
+        intake.setDefaultCommand(new RunCommand(
+                () -> {
+                    intake.setPrimaryIntakeVelocity(0);
+                    intake.setSecondaryIntakeVelocity(0);
+                }, intake
+        ));
+
+        arm.setDefaultCommand(new RunCommand(
+                () -> arm.setArmPosition(arm.getArmPosition()),
+                arm
+        ));
+
         // Initialize autonomous container
         AutonomousContainer.getInstance().setDebugPrints(true);
         AutonomousContainer.getInstance().initialize(
@@ -72,7 +127,7 @@ public RobotContainer() {
                         drive
                 ),
                 drive::resetOdometry,
-                false,
+                true,
                 this
         );
 
@@ -81,24 +136,87 @@ 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(
+            () -> arm.setArmPosition(pickupAngle.get()), arm)
+            .until(() -> Math.abs(arm.getArmPosition() - pickupAngle.get()) < 5)
+            .andThen(() -> {
+                intake.setPrimaryIntakeVelocity(intakeSpeed.get());
+                intake.setSecondaryIntakeVelocity(intakeSpeed.get());
+            }, intake, arm)
+            .until(intake::isBeamBroken);
+
+    @AutoBuilderAccessible
+    Command outtakeCommand = new RunCommand(
+            () -> {
+                intake.setPrimaryIntakeVelocity(ejectSpeed.get());
+                intake.setSecondaryIntakeVelocity(ejectSpeed.get());
+            }, intake
+    );
+
+    @AutoBuilderAccessible
+    Command shootCommand = new RunCommand(
+            () -> {
+                intake.setPrimaryIntakeVelocity(shootSpeed.get());
+                arm.setArmPosition(shootAngle.get());
+            }, intake, arm)
+            .until(() -> Math.abs(arm.getArmPosition() - shootAngle.get()) < 5
+                    && Math.abs(intake.getPrimaryIntakeVelocity() - shootSpeed.get()) < 100)
+            .andThen(() -> {
+                intake.setSecondaryIntakeVelocity(3000);
+                intake.setPrimaryIntakeVelocity(shootSpeed.get());
+                arm.setArmPosition(shootAngle.get());
+            }, intake, arm);
+
+    @AutoBuilderAccessible
+    Command scoreAmp = new RunCommand(
+            () -> arm.setArmPosition(ampScoringAngle.get()), arm)
+            .until(() -> Math.abs(arm.getArmPosition() - ampScoringAngle.get()) < 5)
+            .andThen(() -> {
+                intake.setPrimaryIntakeVelocity(ampScoringSpeed.get());
+                intake.setSecondaryIntakeVelocity(ampScoringSpeed.get());
+            }, intake, arm);
+
+    @AutoBuilderAccessible
+    Command climbCommand = new StartEndCommand(
+            () -> climber.setClimberPower(-0.5), () -> climber.stopClimber(), climber
+    );
+
+    @AutoBuilderAccessible
+    Command stopClimbCommand = new RunCommand(
+            () -> climber.stopClimber(), climber
+    );
+
+    @AutoBuilderAccessible
+    Command deployClimberCommand = new StartEndCommand(
+            () -> climber.extendClimber(), () -> climber.setClimberPower(0), climber
+    );
+
+
     /**
      * 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}),
      * and then passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
      */
     private void configureButtonBindings() {
-
         drive.setDefaultCommand(
                 new RunCommand(() -> drive.driveArcade(-controller.getLeftY(), -controller.getRightX()), drive));
         controller.leftBumper()
                 .whileTrue(new RunCommand(
                         () -> drive.driveArcade(-controller.getLeftY() / 2.0, -controller.getRightX() / 2.0),
                         drive));
+
+        controller.rightTrigger().whileTrue(intakeCommand);
+        controller.leftTrigger().whileTrue(outtakeCommand);
+
+        controller.a().whileTrue(scoreAmp);
+        controller.b().whileTrue(shootCommand);
+
+        controller.povUp().whileTrue(deployClimberCommand);
+        controller.povDown().whileTrue(climbCommand);
     }
 
     /**

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

@@ -0,0 +1,40 @@
+package frc.robot.subsystems.arm;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class Arm extends SubsystemBase {
+
+    ArmIO io;
+    ArmInputsAutoLogged inputs = new ArmInputsAutoLogged();
+
+    public Arm(ArmIO io) {
+        this.io = io;
+    }
+
+    private double targetPosition = 0.0;
+    private final ArmFeedforward feedforward = new ArmFeedforward(0.0, 0.0, 0.0);
+
+    /**
+     * @param position The position to set the arm to in degrees
+     */
+    public void setArmPosition(double position) {
+        targetPosition = position;
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+        Logger.getInstance().processInputs("Arm", inputs);
+
+        double armffvoltage = feedforward.calculate(inputs.armPositionRad, inputs.armVelocityRadPerSec);
+        Logger.getInstance().recordOutput("ArmFFVoltage", armffvoltage);
+
+        io.setArmPosition(targetPosition, armffvoltage);
+    }
+
+    public double getArmPosition() {
+        return inputs.armPositionRad;
+    }
+}

src/main/java/frc/robot/subsystems/arm/ArmIO.java

@@ -0,0 +1,25 @@
+package frc.robot.subsystems.arm;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ArmIO {
+    @AutoLog
+    public static class ArmInputs {
+        public double armPositionRad = 0.0;
+        public double armVelocityRadPerSec = 0.0;
+        public double armCurrent = 0.0;
+        public double armTemperature = 0.0;
+        public double armVoltage = 0.0;
+    }
+
+    /**
+     * Updates the set of loggable inputs.
+     */
+    public default void updateInputs(ArmInputs inputs) {
+
+    }
+
+    public default void setArmPosition(double position, double ffvoltage) {
+
+    }
+}

src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java

@@ -0,0 +1,43 @@
+package frc.robot.subsystems.arm;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import com.revrobotics.SparkMaxPIDController.ArbFFUnits;
+
+public class ArmIOSparkMax implements ArmIO {
+
+    CANSparkMax arm;
+    CANSparkMax armFollower;
+
+    public ArmIOSparkMax() {
+        arm = new CANSparkMax(30, MotorType.kBrushless);
+        arm.getEncoder().setPositionConversionFactor((1.0 / 100.0) * (16.0 / 62.0));
+        arm.getEncoder().setVelocityConversionFactor((1.0 / 100.0) * (16.0 / 62.0) * (1.0 / 60.0) * 2.0 * Math.PI);
+        arm.enableVoltageCompensation(11.0);
+        arm.setSmartCurrentLimit(40);
+
+        arm.getPIDController().setP(0.01);
+        arm.getPIDController().setI(0.0);
+        arm.getPIDController().setD(0.0);
+        arm.getPIDController().setFF(0.0);
+
+        arm.setSmartCurrentLimit(40);
+
+        armFollower = new CANSparkMax(31, MotorType.kBrushless);
+        armFollower.follow(arm, true);
+
+    }
+
+    public void updateInputs(ArmInputs inputs) {
+        inputs.armPositionRad = arm.getEncoder().getPosition();
+        inputs.armVelocityRadPerSec = arm.getEncoder().getVelocity();
+        inputs.armCurrent = arm.getOutputCurrent();
+        inputs.armTemperature = arm.getMotorTemperature();
+        inputs.armVoltage = arm.getBusVoltage();
+    }
+
+    public void setArmPosition(double position, double ffvoltage) {
+        arm.getPIDController().setReference(position, CANSparkMax.ControlType.kPosition, 0,
+                ffvoltage, ArbFFUnits.kVoltage);
+    }
+}