diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7dddebb..dfbdfde 100644 --- a/src/main/java/frc/robot/Constants.java +++ 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); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index d101ae6..672bf1b 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -9,11 +9,11 @@ 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; @@ -21,8 +21,6 @@ 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,19 +59,10 @@ 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() { @@ -81,6 +70,24 @@ public boolean isIntendedAngle() { <= 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(