BallChucker9000 + Limelight Command Based

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

@@ -8,10 +8,14 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.VictorSP;
 import edu.wpi.first.wpilibj.XboxController;
+import frc.robot.commands.BallChucker9000AutoRotate;
 import frc.robot.commands.ExampleCommand;
+import frc.robot.subsystems.BallChucker9000;
 import frc.robot.subsystems.ExampleSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
+import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
 
 /**
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
@@ -20,19 +24,35 @@
  * (including subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
-  // The robot's subsystems and commands are defined here...
-  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
-
-  private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem);
-
+  private final BallChucker9000 BallChucker9000Subsystem;
+  private final BallChucker9000AutoRotate BallChucker9000AutoRotateCommand;
 
+  private final WPI_VictorSPX rotatorMotor;
+  private final VictorSP leftFlywheelMotor;
+  private final VictorSP rightFlywheelMotor;
+  private final WPI_VictorSPX indexerMotor;
 
   /**
    * The container for the robot.  Contains subsystems, OI devices, and commands.
    */
   public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
+
+    rotatorMotor = new WPI_VictorSPX(4);
+    leftFlywheelMotor = new VictorSP(0);
+    rightFlywheelMotor = new VictorSP(1);
+    indexerMotor = new WPI_VictorSPX(31);
+
+    rotatorMotor.setInverted(true);
+    leftFlywheelMotor.setInverted(true);
+    rightFlywheelMotor.setInverted(true);
+    indexerMotor.setInverted(true);
+
+    BallChucker9000Subsystem = new BallChucker9000(new WPI_VictorSPX(4), new VictorSP(0), new VictorSP(1),
+                               new WPI_VictorSPX(31), 0, 1, 7, 8, 2);
+
+    BallChucker9000AutoRotateCommand = new BallChucker9000AutoRotate(BallChucker9000Subsystem);
   }
 
   /**
@@ -52,6 +72,6 @@ private void configureButtonBindings() {
    */
   public Command getAutonomousCommand() {
     // An ExampleCommand will run in autonomous
-    return m_autoCommand;
+    return BallChucker9000AutoRotateCommand;
   }
 }

src/main/java/frc/robot/commands/BallChucker9000AutoRotate.java

@@ -0,0 +1,127 @@
+package frc.robot.commands;
+
+import frc.robot.subsystems.BallChucker9000;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class BallChucker9000AutoRotate extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private final BallChucker9000 BallChucker9000Subsystem;
+
+  private final NetworkTable limelightData = NetworkTableInstance.getDefault().getTable("limelight");
+
+  private NetworkTableEntry tx = limelightData.getEntry("tx"); // X Offset from crosshair
+  private NetworkTableEntry ta = limelightData.getEntry("ta"); // Target's size
+  private NetworkTableEntry tv = limelightData.getEntry("tv"); // Target available
+
+  private NetworkTableEntry ledMode = limelightData.getEntry("ledMode"); // Limelight's LED mode
+
+  private double targetXOffset;
+  private double targetSize;
+  private boolean targetAvailable;
+
+  private double rotatorAngle;
+  private double rotatorDestination;
+
+  /**
+   * Creates command to make the BallChucker9000 automatically target
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public BallChucker9000AutoRotate(BallChucker9000 subsystem) {
+    BallChucker9000Subsystem = subsystem;
+    addRequirements(BallChucker9000Subsystem);
+  }
+
+  @Override
+  public void initialize() {
+
+      ledMode.setNumber(3); // Turn the LEDs on
+
+  }
+
+  @Override
+  public void execute() {
+
+      // Read data
+      targetXOffset = tx.getDouble(0.0);
+      targetSize = ta.getDouble(0.0);
+      targetAvailable = tv.getBoolean(false);
+
+      // Show data (can be removed)
+      SmartDashboard.putNumber("Limelight X Offset", targetXOffset);
+      SmartDashboard.putNumber("Limelight Target Area", targetSize);
+      SmartDashboard.putBoolean("Limelight Target Available", targetAvailable);
+
+      if (targetAvailable) { // If a target is detected (Lock state)
+
+        // get the encoder
+        rotatorAngle = BallChucker9000Subsystem.getRotatorAngle();
+
+        // Set the destnation to the current position to bypass the 
+        // search check the first time it runs
+        rotatorDestination = Math.round(rotatorAngle);
+
+        // Move the rotator to the detected position including offset
+        BallChucker9000Subsystem.rotatorPIDControl(targetXOffset + rotatorAngle);
+
+      } else { // If no target is detected (Search state)
+
+        // get the encoder and round it
+        rotatorAngle = Math.round(BallChucker9000Subsystem.getRotatorAngle());
+
+        if (rotatorDestination == rotatorAngle) { // If we've reached our intended destination, check how we move
+
+          if (rotatorAngle >= 120) {
+
+            // Rotator is at max right rotation, go to max left rotation
+            BallChucker9000Subsystem.rotatorPIDControl(0);
+            rotatorDestination = 0;
+
+          } else if (rotatorAngle >= 0) {
+
+            // Rotator is above or equal to center, go to max right rotation
+            BallChucker9000Subsystem.rotatorPIDControl(120);
+            rotatorDestination = 120;
+
+          } else if (rotatorAngle <= -120) {
+
+            // Rotator is at max left rotation, go to max right rotation
+            BallChucker9000Subsystem.rotatorPIDControl(120);
+            rotatorDestination = 120;
+
+          } else if (rotatorAngle < 0) {
+
+            // Rotator is below center, go to max left rotation
+            BallChucker9000Subsystem.rotatorPIDControl(0);
+            rotatorDestination = 0;
+
+          } else {
+
+            // Uh oh! Hopefully we didn't break anything!
+            // (go to center)
+            BallChucker9000Subsystem.rotatorPIDControl(0);
+            rotatorDestination = 0;
+
+          } 
+
+        }
+
+      }
+
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    BallChucker9000Subsystem.rotatorPIDControl(0); // Reset to resting position
+    ledMode.setNumber(1); // Turn the LEDs off
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/BallChucker9000ManualRotate.java

@@ -0,0 +1,50 @@
+package frc.robot.commands;
+
+import frc.robot.subsystems.BallChucker9000;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class BallChucker9000ManualRotate extends InstantCommand {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  private final BallChucker9000 BallChucker9000Subsystem;
+  private boolean rotateLeft;
+  private double rotatorPosition;
+
+  /**
+   * Creates a new BallChucker9000ManualRotate.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public BallChucker9000ManualRotate(BallChucker9000 subsystem, boolean rotateLeft) {
+    BallChucker9000Subsystem = subsystem;
+    addRequirements(BallChucker9000Subsystem);
+
+    this.rotateLeft = rotateLeft;
+  }
+
+  @Override
+  public void initialize() {
+  }
+
+  @Override
+  public void execute() {
+
+    // Prevent rotating if we're at rotational limits or the switch is pressed
+    rotatorPosition = BallChucker9000Subsystem.getRotatorAngle();
+    if (rotatorPosition <= -120.0 || rotatorPosition >= 120.0 || !BallChucker9000Subsystem.getRotatorAtZeroSwitch()) {
+      BallChucker9000Subsystem.rotatorMotorControl(0);
+    } else {
+      if (this.rotateLeft) {
+        BallChucker9000Subsystem.rotatorMotorControl(-0.25);
+      } else {
+        BallChucker9000Subsystem.rotatorMotorControl(0.25);
+      }
+     }
+
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    BallChucker9000Subsystem.rotatorMotorControl(0.0);
+  }
+}

src/main/java/frc/robot/commands/BallChucker9000Shoot.java

@@ -0,0 +1,37 @@
+package frc.robot.commands;
+
+import frc.robot.subsystems.BallChucker9000;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+
+public class BallChucker9000Shoot extends InstantCommand {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  private final BallChucker9000 BallChucker9000Subsystem;
+  private boolean forward;
+
+  /**
+   * Creates a new BallChucker9000Index.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public BallChucker9000Shoot(BallChucker9000 subsystem, boolean forward) {
+    BallChucker9000Subsystem = subsystem;
+
+    this.forward = forward;
+  }
+
+  @Override
+  public void initialize() {
+    if (this.forward) {
+      BallChucker9000Subsystem.indexerMotorControl(0.75);
+    } else {
+      BallChucker9000Subsystem.indexerMotorControl(-0.75);
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    BallChucker9000Subsystem.indexerMotorControl(0);
+  }
+}

src/main/java/frc/robot/commands/BallChucker9000SpinUp.java

@@ -0,0 +1,40 @@
+package frc.robot.commands;
+
+import frc.robot.subsystems.BallChucker9000;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+
+public class BallChucker9000SpinUp extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private final BallChucker9000 BallChucker9000Subsystem;
+
+  /**
+   * Creates a new BallChucker9000Shoot.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public BallChucker9000SpinUp(BallChucker9000 subsystem) {
+    BallChucker9000Subsystem = subsystem;
+
+    addRequirements(BallChucker9000Subsystem);
+  }
+
+  @Override
+  public void initialize() {
+
+  }
+
+  @Override
+  public void execute() {
+
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}