Merge pull request #26 from Robocubs:shooter-commands

Shooter-commands mergey yay happy dan
Robocubs · Jan 27, 2024 · 0bce456 · 0bce456
2 parents 0ed8664 + 3082bac
commit 0bce456
Show file tree

Hide file tree

Showing 27 changed files with 817 additions and 206 deletions.
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -18,9 +18,11 @@
     {
       "name": "WPIlibUnitTests",
       "workingDirectory": "${workspaceFolder}/build/jni/release",
-      "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ],
+      "vmargs": [
+        "-Djava.library.path=${workspaceFolder}/build/jni/release"
+      ],
       "env": {
-        "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" ,
+        "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release",
         "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release"
       }
     },
@@ -50,6 +52,7 @@
   "cSpell.words": [
     "Brushless",
     "Deadband",
+    "digitalinputs",
     "dtheta",
     "Holonomic",
     "Odometry",
@@ -58,6 +61,7 @@
     "Setpoints",
     "teleop",
     "Uncomitted",
+    "wpilibj",
     "WPILOG"
   ]
 }
diff --git a/build.gradle b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.2.1"
     id "com.diffplug.spotless" version "6.23.3"
     id "com.peterabeles.gversion" version "1.10.2"
 }

diff --git a/src/main/java/com/team1701/lib/drivers/digitalinputs/DigitalIO.java b/src/main/java/com/team1701/lib/drivers/digitalinputs/DigitalIO.java
@@ -0,0 +1,14 @@
+package com.team1701.lib.drivers.digitalinputs;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface DigitalIO {
+    @AutoLog
+    public static class DigitalInputs {
+        public boolean blocked;
+    }
+
+    public default void updateInputs(DigitalInputs inputs) {}
+
+    public default void getBlocked() {}
+}
diff --git a/src/main/java/com/team1701/lib/drivers/digitalinputs/DigitalIOSensor.java b/src/main/java/com/team1701/lib/drivers/digitalinputs/DigitalIOSensor.java
@@ -0,0 +1,16 @@
+package com.team1701.lib.drivers.digitalinputs;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+
+public class DigitalIOSensor implements DigitalIO {
+    private final DigitalInput mSensor;
+
+    public DigitalIOSensor(int channel) {
+        mSensor = new DigitalInput(channel);
+    }
+
+    @Override
+    public void updateInputs(DigitalInputs inputs) {
+        inputs.blocked = mSensor.get();
+    }
+}
diff --git a/src/main/java/com/team1701/lib/drivers/digitalinputs/DigitalIOSim.java b/src/main/java/com/team1701/lib/drivers/digitalinputs/DigitalIOSim.java
@@ -0,0 +1,16 @@
+package com.team1701.lib.drivers.digitalinputs;
+
+import java.util.function.Supplier;
+
+public class DigitalIOSim implements DigitalIO {
+    private final Supplier<Boolean> mBlockedSupplier;
+
+    public DigitalIOSim(Supplier<Boolean> blockedSupplier) {
+        mBlockedSupplier = blockedSupplier;
+    }
+
+    @Override
+    public void updateInputs(DigitalInputs inputs) {
+        inputs.blocked = mBlockedSupplier.get();
+    }
+}
diff --git a/src/main/java/com/team1701/lib/drivers/encoders/EncoderIODigital.java b/src/main/java/com/team1701/lib/drivers/encoders/EncoderIODigital.java
@@ -0,0 +1,30 @@
+package com.team1701.lib.drivers.encoders;
+
+import com.team1701.robot.Constants;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+
+public class EncoderIODigital implements EncoderIO {
+    private final DutyCycleEncoder mEncoder;
+
+    public EncoderIODigital(int channel) {
+        mEncoder = new DutyCycleEncoder(Constants.Shooter.kShooterThroughBoreEncoderId);
+        mEncoder.setDistancePerRotation(Constants.Shooter.kThroughBoreEncoderDistancePerRotation);
+    }
+
+    @Override
+    public void updateInputs(EncoderInputs inputs) {
+        var rotations = mEncoder.get();
+        inputs.position = Rotation2d.fromRotations(
+                rotations == Double.NaN || rotations == Double.POSITIVE_INFINITY ? 0.0 : rotations);
+    }
+
+    public void setPositionOffset(Rotation2d offset) {
+        mEncoder.setPositionOffset(MathUtil.inputModulus(offset.getRotations(), 0.0, 1.0));
+    }
+
+    public void setDistancePerRotation(double distancePerRotation) {
+        mEncoder.setDistancePerRotation(distancePerRotation);
+    }
+}
diff --git a/src/main/java/com/team1701/robot/Configuration.java b/src/main/java/com/team1701/robot/Configuration.java
@@ -34,8 +34,12 @@ public static Mode getMode() {
         }
     }
 
-    public static Alliance getAlliance() {
-        return DriverStation.getAlliance().orElse(Alliance.Blue);
+    public static boolean isBlueAlliance() {
+        return DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue;
+    }
+
+    public static boolean isRedAlliance() {
+        return DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;
     }
 
     public static enum RobotType {

diff --git a/src/main/java/com/team1701/robot/Constants.java b/src/main/java/com/team1701/robot/Constants.java
@@ -158,26 +158,47 @@ public static final class Drive {
     public static final class Shooter {
         // TODO: Update values
         public static final double kShooterReduction = 1;
-        public static final int kShooterDeviceId = 0;
+        public static final int kShooterUpperRollerMotorId = 0;
+        public static final int kShooterLowerRollerMotorId = 1;
+        public static final int kShooterRotationMotorId = 2;
 
         public static final double kShooterAxisHeight = Units.inchesToMeters(10);
         public static final double kShooterAxisOffset = Units.inchesToMeters(10); // + is toward front of bot
 
-        public static final LoggedTunableNumber kShooterKff = new LoggedTunableNumber("Shooter/Motor/Kff");
-        public static final LoggedTunableNumber kShooterKp = new LoggedTunableNumber("Shooter/Motor/Kp");
-        public static final LoggedTunableNumber kShooterKd = new LoggedTunableNumber("Shooter/Motor/Kd");
+        public static final LoggedTunableNumber kRollerKff = new LoggedTunableNumber("Shooter/Motor/Roller/Kff");
+        public static final LoggedTunableNumber kRollerKp = new LoggedTunableNumber("Shooter/Motor/Roller/Kp");
+        public static final LoggedTunableNumber kRollerKd = new LoggedTunableNumber("Shooter/Motor/Roller/Kd");
+
+        public static final LoggedTunableNumber kRotationKff = new LoggedTunableNumber("Shooter/Motor/Rotation/Kff");
+        public static final LoggedTunableNumber kRotationKp = new LoggedTunableNumber("Shooter/Motor/Rotation/Kp");
+        public static final LoggedTunableNumber kRotationKd = new LoggedTunableNumber("Shooter/Motor/Rotation/Kd");
+
+        public static int kShooterEntranceSensorId;
+        public static int kShooterExitSensorId;
+        public static int kShooterThroughBoreEncoderId;
+
+        public static double kThroughBoreEncoderDistancePerRotation;
 
         static {
             switch (Configuration.getRobot()) {
                 case COMPETITION_BOT:
-                    kShooterKd.initDefault(1);
-                    kShooterKp.initDefault(0.6);
-                    kShooterKff.initDefault(0);
+                    kRollerKd.initDefault(1);
+                    kRollerKp.initDefault(0.6);
+                    kRollerKff.initDefault(0);
+
+                    kRotationKd.initDefault(1);
+                    kRotationKp.initDefault(0.6);
+                    kRotationKff.initDefault(0);
+
                     break;
                 case SIMULATION_BOT:
-                    kShooterKd.initDefault(1);
-                    kShooterKp.initDefault(0.6);
-                    kShooterKff.initDefault(0);
+                    kRollerKd.initDefault(1);
+                    kRollerKp.initDefault(0.6);
+                    kRollerKff.initDefault(0);
+
+                    kRotationKd.initDefault(1);
+                    kRotationKp.initDefault(0.6);
+                    kRotationKff.initDefault(0);
                     break;
                 default:
                     throw new UnsupportedOperationException("No shooter configuration for " + Configuration.getRobot());

diff --git a/src/main/java/com/team1701/robot/FieldConstants.java b/src/main/java/com/team1701/robot/FieldConstants.java
@@ -4,11 +4,15 @@
 import edu.wpi.first.math.util.Units;
 
 public final class FieldConstants {
-    // blue locations use blue field origin, red locations use red origin
-    // all units are in meters and radians
     public static double kFieldLongLengthMeters = (Units.inchesToMeters((54 * 12) + 3.25));
     public static double kFieldShortLengthMeters = (Units.inchesToMeters((26 * 12) + 11.25));
+    public static double kCenterLine = kFieldLongLengthMeters / 2.0;
+    public static double kWingLength = Units.inchesToMeters(231.2);
 
-    public static Translation3d kBlueSpeakerOpeningCenter = new Translation3d(0, Units.inchesToMeters(218.42), 204.5);
-    public static Translation3d kRedSpeakerOpeningCenter = new Translation3d(0, Units.inchesToMeters(651.23), 204.5);
+    public static Translation3d kBlueSpeakerOpeningCenter =
+            new Translation3d(Units.inchesToMeters(0.9), Units.inchesToMeters(218.42), 204.5);
+    public static Translation3d kRedSpeakerOpeningCenter = new Translation3d(
+            kFieldLongLengthMeters - kBlueSpeakerOpeningCenter.getX(),
+            kBlueSpeakerOpeningCenter.getY(),
+            kBlueSpeakerOpeningCenter.getZ());
 }