Add waggle stick subsystem

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

@@ -10,6 +10,7 @@
 import org.lasarobotics.drive.MAXSwerveModule;
 import org.lasarobotics.hardware.kauailabs.NavX2;
 import org.lasarobotics.hardware.revrobotics.Spark;
+import org.lasarobotics.hardware.revrobotics.SparkPIDConfig;
 import org.lasarobotics.led.LEDStrip;
 import org.lasarobotics.utils.PIDConstants;
 
@@ -20,6 +21,7 @@
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.units.Units;
 import frc.robot.subsystems.drive.PurplePathPose;
 import frc.robot.subsystems.vision.AprilTagCamera.Resolution;
@@ -123,6 +125,19 @@ public static class VisionHardware {
     public static final Rotation2d CAMERA_OBJECT_FOV = Rotation2d.fromDegrees(79.7);
   }
 
+  public static class WiggleStick {
+    public static final SparkPIDConfig WIGGLE_STICK_CONFIG = new SparkPIDConfig(
+      new PIDConstants(0.2, 0, 0, 0),
+      false,
+      false,
+      1.0,
+      0.0,
+      15.0,
+      true
+    );
+    public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(10, 20);
+  }
+
   public static class SmartDashboard {
     public static final String SMARTDASHBOARD_DEFAULT_TAB = "SmartDashboard";
     public static final String SMARTDASHBOARD_AUTO_MODE = "Auto Mode";

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

@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.subsystems.WaggleSubsystem;
 import frc.robot.subsystems.drive.DriveSubsystem;
 
 public class RobotContainer {
@@ -24,6 +25,8 @@ public class RobotContainer {
     Constants.Drive.DRIVE_LOOKAHEAD
   );
 
+  private static final WaggleSubsystem WIGGLE_STICK = new WaggleSubsystem(Constants.WiggleStick.WIGGLE_STICK_CONFIG, Constants.WiggleStick.CONSTRAINTS);
+
   private static final CommandXboxController PRIMARY_CONTROLLER = new CommandXboxController(Constants.HID.PRIMARY_CONTROLLER_PORT);
 
   public RobotContainer() {
@@ -58,6 +61,10 @@ private void configureBindings() {
     PRIMARY_CONTROLLER.b().whileTrue(DRIVE_SUBSYSTEM.goToPoseCommand(Constants.Field.SOURCE));
 
     PRIMARY_CONTROLLER.povLeft().onTrue(DRIVE_SUBSYSTEM.resetPoseCommand(() -> new Pose2d()));
+
+    // POV left/right - wiggle stick
+    PRIMARY_CONTROLLER.povLeft().onTrue(WIGGLE_STICK.setPositionCommand(0.0));
+    PRIMARY_CONTROLLER.povRight().onTrue(WIGGLE_STICK.setPositionCommand(15.0));
   }
 
   /**

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

@@ -0,0 +1,58 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import org.lasarobotics.hardware.revrobotics.Spark;
+import org.lasarobotics.hardware.revrobotics.Spark.FeedbackSensor;
+import org.lasarobotics.hardware.revrobotics.Spark.MotorKind;
+import org.lasarobotics.hardware.revrobotics.SparkPIDConfig;
+
+import com.revrobotics.CANSparkBase.IdleMode;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+
+public class WaggleSubsystem extends SubsystemBase {
+  Spark m_motor;
+  SparkPIDConfig m_config;
+  Constraints m_constraint;
+
+  /** Creates a new wiggleStick. */
+  public WaggleSubsystem(SparkPIDConfig config, Constraints constraint) {
+    m_motor = new Spark(new Spark.ID("wiggleStick", 20), MotorKind.NEO);
+    m_config = config;
+    m_constraint = constraint;
+
+    double conversionFactor = 1.0;
+    m_motor.initializeSparkPID(config, FeedbackSensor.NEO_ENCODER);
+    m_motor.setPositionConversionFactor(FeedbackSensor.NEO_ENCODER, conversionFactor);
+    m_motor.setVelocityConversionFactor(FeedbackSensor.NEO_ENCODER, conversionFactor);
+
+    m_motor.enablePIDWrapping(0.0, 15.0);
+    m_motor.setIdleMode(IdleMode.kCoast);
+    m_motor.resetEncoder();
+  }
+
+  public void setPosition(double position) {
+    m_motor.smoothMotion(position, m_constraint);
+  }
+
+  public double getPosition() {
+    return m_motor.getInputs().encoderPosition;
+  }
+
+  public Command setPositionCommand(double position) {
+    return runOnce(() -> setPosition(position));
+
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    m_motor.periodic();
+  }
+}