Merge branch 'main' of https://github.com/team581/offseason-2023-bot-1

team581 · Aug 26, 2023 · ead01f1 · ead01f1
2 parents 1ed3d38 + f38d7e5
commit ead01f1
Show file tree

Hide file tree

Showing 4 changed files with 129 additions and 66 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -34,8 +34,7 @@ public class Robot extends LoggedRobot {
 
   private final DriveController driveController = new DriveController(0);
   private final CommandXboxController operatorController = new CommandXboxController(1);
-  private final RumbleControllerSubsystem rumbleController =
-      new RumbleControllerSubsystem(new XboxController(1));
+  private final RumbleControllerSubsystem rumbleController = new RumbleControllerSubsystem(new XboxController(1));
 
   private final FmsSubsystem fmsSubsystem = new FmsSubsystem();
 
@@ -44,6 +43,9 @@ public class Robot extends LoggedRobot {
   private Command autoCommand;
 
   public Robot() {
+
+    //XboxController.x().onTrue(wrist.setPositionCommand(10));
+
     // Log to a USB stick
     Logger.getInstance().addDataReceiver(new WPILOGWriter("/media/sda1/"));
     if (Config.IS_DEVELOPMENT) {

diff --git a/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java b/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java
@@ -9,6 +9,7 @@ public enum SubsystemPriority {
   AUTOBALANCE(11),
   AUTOROTATE(11),
 
+  WRIST(10),
   SWERVE(10),
   IMU(10),
   INTAKE(10),

diff --git a/src/main/java/frc/robot/wrist/WristSubsystem.java b/src/main/java/frc/robot/wrist/WristSubsystem.java
@@ -0,0 +1,54 @@
+package frc.robot.wrist;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.ControlType;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxPIDController;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.util.scheduling.LifecycleSubsystem;
+import frc.robot.util.scheduling.SubsystemPriority;
+
+public class WristSubsystem extends LifecycleSubsystem {
+  private final CANSparkMax motor;
+  private RelativeEncoder encoder;
+  private SparkMaxPIDController pid;
+  private double goalAngle = 0.0;
+
+  public WristSubsystem(CANSparkMax motor) {
+    super(SubsystemPriority.WRIST);
+
+    this.motor = motor;
+    encoder = motor.getEncoder();
+    pid = motor.getPIDController();
+
+    pid.setP(5);
+    pid.setI(0);
+    pid.setD(0);
+
+    encoder.setPosition(0);
+    encoder.setPositionConversionFactor(50);
+  }
+
+  @Override
+  public void enabledPeriodic() {
+    pid.setReference(goalAngle / 360.0, ControlType.kSmartMotion);
+  }
+
+  public void set(double angle) {
+    goalAngle = angle;
+  }
+
+  public Command setPositionCommand(double angle) {
+    return run(() -> {
+          set(angle);
+        })
+        .until(() -> atAngle(angle));
+  }
+
+  private boolean atAngle(double angle) {
+    return Math.abs(getWristAngle() - angle) < 1;
+  }
+
+  private double getWristAngle() {
+    return encoder.getPosition() * 360.0;
+  }
+}
diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json
@@ -1,67 +1,73 @@
 {
-  "javaDependencies": [{
-    "groupId": "com.revrobotics.frc",
-    "artifactId": "REVLib-java",
-    "version": "2023.1.3"
-  }],
-  "fileName": "REVLib.json",
-  "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json",
-  "name": "REVLib",
-  "jniDependencies": [{
-    "validPlatforms": [
-      "windowsx86-64",
-      "windowsx86",
-      "linuxarm64",
-      "linuxx86-64",
-      "linuxathena",
-      "linuxarm32",
-      "osxuniversal"
+    "fileName": "REVLib.json",
+    "name": "REVLib",
+    "version": "2023.1.3",
+    "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+    "mavenUrls": [
+        "https://maven.revrobotics.com/"
     ],
-    "groupId": "com.revrobotics.frc",
-    "artifactId": "REVLib-driver",
-    "skipInvalidPlatforms": true,
-    "isJar": false,
-    "version": "2023.1.3"
-  }],
-  "mavenUrls": ["https://maven.revrobotics.com/"],
-  "cppDependencies": [
-    {
-      "groupId": "com.revrobotics.frc",
-      "libName": "REVLib",
-      "artifactId": "REVLib-cpp",
-      "skipInvalidPlatforms": true,
-      "version": "2023.1.3",
-      "headerClassifier": "headers",
-      "binaryPlatforms": [
-        "windowsx86-64",
-        "windowsx86",
-        "linuxarm64",
-        "linuxx86-64",
-        "linuxathena",
-        "linuxarm32",
-        "osxuniversal"
-      ],
-      "sharedLibrary": false
-    },
-    {
-      "groupId": "com.revrobotics.frc",
-      "libName": "REVLibDriver",
-      "artifactId": "REVLib-driver",
-      "skipInvalidPlatforms": true,
-      "version": "2023.1.3",
-      "headerClassifier": "headers",
-      "binaryPlatforms": [
-        "windowsx86-64",
-        "windowsx86",
-        "linuxarm64",
-        "linuxx86-64",
-        "linuxathena",
-        "linuxarm32",
-        "osxuniversal"
-      ],
-      "sharedLibrary": false
-    }
-  ],
-  "version": "2023.1.3",
-  "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb"
+    "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-java",
+            "version": "2023.1.3"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-driver",
+            "version": "2023.1.3",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-cpp",
+            "version": "2023.1.3",
+            "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": "2023.1.3",
+            "libName": "REVLibDriver",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ]
 }