Feature/intake

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

@@ -13,6 +13,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Distance;
@@ -22,20 +23,25 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants.AutoConstants;
 import frc.robot.Constants.Fixtures;
+import frc.robot.Constants.IntakeConstants;
 import frc.robot.Constants.OIConstants;
+import frc.robot.Constants.ShooterConstants;
 import frc.robot.commands.AimCommandFactory;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
-import frc.robot.utils.TargetSolution;
-import frc.robot.utils.UtilityFunctions;
+import frc.robot.utils.*;
 
 public class RobotContainer {
 
+    private final IntakeSubsystem m_intake = new IntakeSubsystem();
+
     private final CommandXboxController m_driverController = new CommandXboxController(
             OIConstants.kDriverControllerPort);
 
@@ -57,9 +63,11 @@ public RobotContainer() {
         m_chooser.setDefaultOption("Example Auto", AutoConstants.kExampleAutoName);
         SmartDashboard.putData("Auto Choices", m_chooser);
 
-        SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0);
-        SmartDashboard.putNumber("Shooter hood angle in degrees", 0.0);
-        SmartDashboard.putNumber("Turret angle in degrees", 0.0);
+        NetworkTableInstance inst = NetworkTableInstance.getDefault();
+
+        // SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0);
+        // SmartDashboard.putNumber("Shooter hood angle in degrees", 0.0);
+        // SmartDashboard.putNumber("Turret angle in degrees", 0.0);
 
         // Configure the button bindings
         configureBindings();
@@ -83,14 +91,32 @@ private void configureBindings() {
         // m_driverController.a()
         // .whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40)));
 
-        m_driverController.b()
-                .whileTrue(m_aimFactory.Aim(Degrees.of(SmartDashboard.getNumber("Turret angle in degrees", 0.0)),
-                        Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0))));
+        // m_driverController.b()
+        // .whileTrue(m_aimFactory.Aim(Degrees.of(SmartDashboard.getNumber("Turret angle
+        // in degrees", 0.0)),
+        // Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0))));
+
+        // m_driverController.a().whileTrue(m_aimFactory.ShootCommand());
+
+        // System.out.println("Bindings configured");
+        // m_driverController.x().whileTrue(m_aimFactory.PointAtHub(true));
+
+        // m_driverController.x().whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40)));
 
-        m_driverController.a().whileTrue(m_aimFactory.ShootCommand());
+        // m_driverController.x().whileTrue(
+        // m_aimFactory.Shoot(ShooterConstants.kFeedingWheelVelocity)
+        // .finallyDo(() -> m_aimFactory.Shoot(RPM.of(0.0))));
+
+        // m_driverController.y().whileTrue(m_aimFactory.RunAllStager());
+
+        m_driverController.x().onTrue(DeployIntake());
+
+        m_driverController.a().onTrue(retractIntake());
+
+        m_driverController.b().whileTrue(spinIntake());
+
+        m_driverController.y().whileTrue(m_aimFactory.RunAllStager());
 
-        System.out.println("Bindings configured");
-        m_driverController.x().whileTrue(m_aimFactory.PointAtHub(true));
     }
 
     public Command getAutonomousCommand() {
@@ -113,6 +139,37 @@ public Command feedPosition(Alliance alliance) {
         }, m_drive, m_turret);
     }
 
+    public Command spinIntake() {
+        return new RunCommand(() -> {
+            m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed);
+        }).finallyDo(m_intake::stopIntake);
+    }
+
+    public Command retractIntake() {
+        return new InstantCommand(() -> {
+            m_intake.retractIntake();
+        });
+    }
+
+    public Command outTake() {
+        return new RunCommand(() -> {
+            m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed.times(-1));
+        });
+    }
+
+    public Command DeployIntake() {
+        return new InstantCommand(() -> {
+            m_intake.deployIntake();
+        });
+
+    }
+
+    public Command SpinIntake() {
+        return new InstantCommand(() -> {
+            m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed);
+        });
+    }
+
     public void teleopPeriodic() {
         m_turret.addDriveHeading(UtilityFunctions.WrapAngle(m_drive.getHeading()));
 

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

@@ -17,6 +17,7 @@
 import static edu.wpi.first.units.Units.Degrees;
 import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.RPM;
 import static edu.wpi.first.units.Units.Radians;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.Seconds;
@@ -40,7 +41,7 @@
 import frc.robot.Constants.TurretConstants;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
-import frc.robot.subsystems.StagingSubsystem;
+import frc.robot.subsystems.*;
 import frc.robot.subsystems.TurretSubsystem;
 import frc.robot.utils.ShootingEntry;
 import frc.robot.utils.TargetSolution;
@@ -54,7 +55,7 @@ public class AimCommandFactory {
 
     private boolean m_isAiming = false;
 
-    private AngularVelocity m_wheelVelocity;
+    private AngularVelocity m_wheelVelocity = RPM.of(60);
 
     private Translation2d m_lockedTag;
 
@@ -134,6 +135,18 @@ public Command ShootCommand() {
                 }, m_stager)).finallyDo(m_shooter::Stop);
     }
 
+    public Command RunAllStager() {
+        return new RunCommand(() -> {
+            m_stager.Agitate();
+            m_stager.Feed();
+            m_stager.Roll();
+        }, m_stager).finallyDo(() -> {
+            m_stager.StopAgitate();
+            m_stager.StopFeed();
+            m_stager.StopRoll();
+        });
+    }
+
     private void Shoot() {
         m_shooter.Spin(m_wheelVelocity);
     }
@@ -159,7 +172,7 @@ public TargetSolution GetHubAimSolution() {
     }
 
     public Command MoveTurretToHeadingCommand(Angle heading) {
-        return new InstantCommand(() -> {
+        return new RunCommand(() -> {
             MoveTurretToHeading(heading);
         }, m_turret);
     }
@@ -191,7 +204,8 @@ public void MoveTurretToHeading(Angle heading) {
 
         Angle robotRelativeTurretAngle = UtilityFunctions.WrapAngle(heading.minus(robotHeading));
 
-        Angle[] currentRange = getCurrentTurretRange();
+        // Angle[] currentRange = getCurrentTurretRange();
+        Angle[] currentRange = TurretConstants.kUnrestrictedAngles;
 
         if (withinAngles(currentRange, robotRelativeTurretAngle)) {
             m_turret.moveToAngle(robotRelativeTurretAngle);
@@ -209,7 +223,7 @@ public void MoveTurretToHeading(Angle heading) {
 
             Angle driveTarget = heading.minus(closest);
 
-            System.out.println();
+            // System.out.println();
             m_drive.moveToAngle(driveTarget);
             m_turret.moveToAngle(closest);
         }
@@ -304,6 +318,10 @@ private static Angle getClosestAngle(Angle a, Angle... others) {
         // System.out.print(a.in(Degrees) + " is robot heading");
         // System.out.println();
 
+        if (others.length == 0) {
+            return null;
+        }
+
         Angle closest = UtilityFunctions.WrapAngle(others[0]);
         double closestDistance = UtilityFunctions.angleDiff(a, closest).abs(Degrees);
 

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -15,6 +15,9 @@
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -268,10 +271,10 @@ public void periodic() {
 
         m_vision.periodic();
 
-        SmartDashboard.putData(m_field);
-
         m_pidController.periodic();
 
+        SmartDashboard.putData(m_field);
+
         SmartDashboard.putBoolean("Is manual rotate", m_isManualRotate);
     }
 
@@ -415,9 +418,9 @@ public void zeroHeading() {
      * @return the robot's heading in degrees, from -180 to 180
      */
     public Angle getHeading() {
-        // return pidgey.getYaw().getValue();
+        return pidgey.getYaw().getValue();
         // TODO: Don't use this code
-        return m_poseEstimator.getEstimatedPosition().getRotation().getMeasure();
+        // return m_poseEstimator.getEstimatedPosition().getRotation().getMeasure();
     }
 
     public Angle getGyroHeading() {