fixed velocity it was very sus. finished intake methods and created b…

src/main/java/frc/robot/Constants.java

@@ -162,6 +162,7 @@ public class HopperConstants {
     public static final String MOTOR_NAME = "Hopper Roller";
 
     // CHANGE TO PROPER RPMS !!!!
+
     public static final double SLOW_SPEED_RPM = 0.0;
     public static final double FAST_SPEED_RPM = 0.0;
     public static final double REVERSE_SPEED_RPM = 0.0;
@@ -437,6 +438,7 @@ public class IntakeFlywheelConstants {
     public static final Angle MIN_ANGLE = Rotations.of(0.0);
     public static final Angle MAX_ANGLE = Rotations.of(1);
     public static final Angle STARTING_ANGLE = Rotations.of(0.0);
+    public static final double PICKUP_SPEED = 0.0;
     public static final Distance WHEEL_RADIUS = Meters.of(0.05);
     public static final Translation3d OFFSET = Translation3d.kZero;
     public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.0028125);
@@ -568,6 +570,9 @@ public static TalonFXConfiguration getFXConfig() {
   public class IntakePivotConstants {
     public static final String NAME = "Intake";
 
+    public static final Angle PICKUP_ANGLE = Rotations.of(0.0);
+    public static final Angle STOW_ANGLE = Rotations.of(0.0);
+
     public static final Angle TOLERANCE = Degrees.of(1.0);
 
     public static final AngularVelocity CRUISE_VELOCITY = RadiansPerSecond.of(10);

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

@@ -9,20 +9,18 @@
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.lib.W8.io.motor.MotorIO.PIDSlot;
 import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism;
 import frc.lib.W8.mechanisms.rotary.RotaryMechanism;
-import frc.robot.Constants;
 import frc.robot.Constants.IntakeFlywheelConstants;
 import frc.robot.Constants.IntakePivotConstants;
 import org.littletonrobotics.junction.Logger;
 
 public class Intake extends SubsystemBase {
   private FlywheelMechanism _rollerIO;
   private RotaryMechanism _pivotIO;
-  double velocity;
-  double pivotAngle;
   public double desiredAngle;
 
   public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) {
@@ -34,7 +32,7 @@ public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) {
   public void setVelocity(double velocity) {
     AngularVelocity angVelo = RotationsPerSecond.of(velocity);
 
-    _rollerIO.runVelocity(angVelo, Constants.IntakeFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0);
+    _rollerIO.runVelocity(angVelo, IntakeFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0);
   }
 
   public Command setPivotAngle(Angle pivotAngle) {
@@ -61,26 +59,35 @@ public void stop() {
     setVelocity(0);
   }
 
-  // public Command intake() {
-  //   return Commands.sequence(
-  //       Commands.run(() -> setVelocity(velocity)), Commands.run(() ->
-  // setPivotAngle(pivotAngle)));
-  // }
-  public void setAngle(Angle angle) {
-    _pivotIO.runPosition(
-        angle,
-        getVelocity(),
-        IntakePivotConstants.ACCELERATION,
-        IntakePivotConstants.JERK,
-        PIDSlot.SLOT_0);
-    desiredAngle = angle.magnitude();
+  public Command intake() {
+    return Commands.sequence(
+        Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)),
+        setPivotAngle(IntakePivotConstants.PICKUP_ANGLE));
   }
 
   public boolean isIntendedAngle() {
     return Math.abs(desiredAngle - _pivotIO.getVelocity().in(RotationsPerSecond))
         <= IntakePivotConstants.TOLERANCE.magnitude();
   }
 
+  public Command stowAndStopRollers() {
+    return Commands.sequence(
+        Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)),
+        setStowAngle(IntakePivotConstants.STOW_ANGLE));
+  }
+
+  private Command setStowAngle(Angle stowAngle) {
+    return this.runOnce(
+        () ->
+            _pivotIO.runPosition(
+                stowAngle,
+                IntakePivotConstants.CRUISE_VELOCITY,
+                IntakePivotConstants.ACCELERATION,
+                IntakePivotConstants.JERK,
+                PIDSlot.SLOT_0));
+  }
+
+  @Override
   public void periodic() {
     _pivotIO.periodic();
     Logger.recordOutput(