Merge pivot to develop (by dror)

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

@@ -15,5 +15,20 @@
 public final class Constants {
   public static class OperatorConstants {
     public static final int kDriverControllerPort = 0;
+
   }
+
+
+  public static final double completeAngle = 360;
+
+  // Variables used by the pivot
+  public static final int pivotMotorID = 1;
+  public static final double pivotTolerance = 2.0;
+
+  public static final double pivotP = 0.4;
+  public static final double pivotI = 0.0;
+  public static final double pivotD = 0.0;
+
+  public static final int pivotMaxVelocity = 1000;
+  public static final int pivotMaxAccel = 1000;
 }

src/main/java/frc/robot/Robot.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.subsystems.Pivot;
 
 /**
  * The methods in this class are called automatically corresponding to each mode, as described in

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

@@ -0,0 +1,45 @@
+// 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.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Pivot;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class PivotMoveToAngle extends Command {
+  private Pivot myPivot;
+  private double myAngle;
+
+  private PivotMoveToAngle(double myAngle)
+  {
+    this.myAngle = myAngle;
+    myPivot = Pivot.getInstance();
+    addRequirements(myPivot);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize()
+  {
+    myPivot.movePivotToAngle(myAngle);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted)
+  {
+    myPivot.stopMotor();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return myPivot.isAtAngle(myAngle);
+  }
+}

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

@@ -0,0 +1,49 @@
+// 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.commands;
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Pivot;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class PivotMoveWithJoystick extends Command {
+  private Pivot myPivot;
+
+  private DoubleSupplier doubleSupplier;
+
+  private PivotMoveWithJoystick(DoubleSupplier myDoubleSupplier)
+  {
+    doubleSupplier = myDoubleSupplier;
+
+    myPivot = Pivot.getInstance();
+    addRequirements(myPivot);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute()
+  {
+    myPivot.setPower(doubleSupplier.getAsDouble());
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted)
+  {
+    myPivot.stopMotor();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,104 @@
+// 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 edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkFlexConfig;
+
+public class Pivot extends SubsystemBase {
+  private SparkFlex motor;
+  private SparkFlexConfig motorConfig;
+  private SparkClosedLoopController closedLoopController;
+  private RelativeEncoder encoder;
+
+  private static Pivot instance = null;
+
+  private Pivot()
+  {
+    motor = new SparkFlex(Constants.pivotMotorID, MotorType.kBrushless);
+    closedLoopController = motor.getClosedLoopController();
+    encoder = motor.getEncoder();
+
+    motorConfig.encoder.positionConversionFactor(Constants.completeAngle);
+
+    motorConfig = new SparkFlexConfig();
+
+    motorConfig.idleMode(IdleMode.kBrake);    
+
+    motorConfig.closedLoop
+        .feedbackSensor(FeedbackSensor.kAlternateOrExternalEncoder)
+        .p(Constants.pivotP)
+        .i(Constants.pivotI)
+        .d(Constants.pivotD)
+        .outputRange(-1, 1);
+
+    motorConfig.closedLoop.maxMotion
+        .maxVelocity(Constants.pivotMaxVelocity)
+        .maxAcceleration(Constants.pivotMaxAccel)
+        .allowedClosedLoopError(1);
+
+    motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
+
+    encoder.setPosition(motor.getAbsoluteEncoder().getPosition());
+  }
+
+  public static Pivot getInstance()
+  {
+    if (instance == null)
+    {
+      instance = new Pivot();
+    }
+
+    return instance;
+  }
+
+  /***
+   * The function moves the pivot to the wanted angle
+   * 
+   * @param angle - the angle to move the motor to
+   */
+  public void movePivotToAngle(double angle)
+  {
+    closedLoopController.setReference(angle, ControlType.kMAXMotionPositionControl,ClosedLoopSlot.kSlot0);
+  }
+
+  public void setPower(double power)
+  {
+    motor.set(power);
+  }
+
+  public void stopMotor()
+  {
+    motor.stopMotor();
+  }
+
+  /***
+   * The function checks if the pivot has reached the wanted angle
+   * 
+   * @param angle - The angle we want the motor to move to
+   * @return if the motor has reached the wanted angle
+   */
+  public boolean isAtAngle(double angle)
+  {
+    return encoder.getPosition() <= angle + Constants.pivotTolerance && encoder.getPosition() >= angle - Constants.pivotTolerance;
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

vendordeps/REVLib-2025.0.0.json

@@ -0,0 +1,74 @@
+{
+    "fileName": "REVLib-2025.0.0.json",
+    "name": "REVLib",
+    "version": "2025.0.0",
+    "frcYear": "2025",
+    "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+    "mavenUrls": [
+        "https://maven.revrobotics.com/"
+    ],
+    "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-java",
+            "version": "2025.0.0"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-driver",
+            "version": "2025.0.0",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-cpp",
+            "version": "2025.0.0",
+            "libName": "REVLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-driver",
+            "version": "2025.0.0",
+            "libName": "REVLibDriver",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ]
+}