Shooter-commands

.vscode/settings.json

@@ -54,13 +54,20 @@
     }
   },
   "eslint.rules.customizations": [
-    { "rule": "*", "severity": "warn" },
-    { "rule": "prettier/prettier", "severity": "off" }
+    {
+      "rule": "*",
+      "severity": "warn"
+    },
+    {
+      "rule": "prettier/prettier",
+      "severity": "off"
+    }
   ],
   "eslint.validate": ["javascript", "typescript", "svelte"],
   "cSpell.words": [
     "Brushless",
     "Deadband",
+    "digitalinputs",
     "dtheta",
     "Holonomic",
     "Odometry",
@@ -70,6 +77,7 @@
     "Setpoints",
     "teleop",
     "Uncomitted",
+    "wpilibj",
     "WPILOG"
   ]
 }

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() {}
+}

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();
+    }
+}

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();
+    }
+}

src/main/java/com/team1701/lib/drivers/encoders/EncoderIOAnalog.java

@@ -5,7 +5,7 @@
 import edu.wpi.first.wpilibj.AnalogEncoder;
 
 public class EncoderIOAnalog implements EncoderIO {
-    private final AnalogEncoder mEncoder;
+    public final AnalogEncoder mEncoder;
 
     public EncoderIOAnalog(int channel) {
         mEncoder = new AnalogEncoder(channel);

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 {
+    public 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);
+    }
+}

src/main/java/com/team1701/lib/drivers/encoders/EncoderIORevThroughBore.java

@@ -0,0 +1,29 @@
+package com.team1701.lib.drivers.encoders;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+
+public class EncoderIORevThroughBore implements EncoderIO {
+    public final DutyCycleEncoder mEncoder;
+
+    public EncoderIORevThroughBore(int channel) {
+        mEncoder = new DutyCycleEncoder(channel);
+        mEncoder.setDistancePerRotation(360);
+    }
+
+    @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);
+    }
+}

src/main/java/com/team1701/lib/util/GeometryUtil.java

@@ -5,6 +5,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 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.geometry.Twist2d;
 
@@ -32,4 +33,15 @@ public static boolean isNear(Rotation2d expected, Rotation2d actual, Rotation2d
         var difference = MathUtil.angleModulus(expected.minus(actual).getRadians());
         return MathUtil.isNear(difference, 0.0, tolerance.getRadians());
     }
+
+    public static boolean isNear(Translation2d expected, Translation2d actual, double tolerance) {
+        var difference = expected.minus(actual).getNorm();
+        return MathUtil.isNear(difference, 0.0, tolerance);
+    }
+
+    public static boolean isNear(
+            Pose2d expected, Pose2d actual, double translationTolerance, Rotation2d rotationTolerance) {
+        return isNear(expected.getTranslation(), actual.getTranslation(), translationTolerance)
+                && isNear(expected.getRotation(), actual.getRotation(), rotationTolerance);
+    }
 }

src/main/java/com/team1701/robot/Constants.java

@@ -6,6 +6,7 @@
 import com.team1701.lib.swerve.ExtendedSwerveDriveKinematics;
 import com.team1701.lib.swerve.SwerveSetpointGenerator.KinematicLimits;
 import com.team1701.lib.util.LoggedTunableNumber;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 
@@ -157,27 +158,56 @@ 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 double kRollerReduction = 1.0;
+        public static final double kAngleReduction = 1.0 / 105.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 final Rotation2d kShooterAngleEncoderOffset;
+
+        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);
+                    kRollerKff.initDefault(0.0);
+                    kRollerKp.initDefault(0.0);
+                    kRollerKd.initDefault(0.0);
+
+                    kRotationKff.initDefault(0.0);
+                    kRotationKp.initDefault(0.0);
+                    kRotationKd.initDefault(0.0);
+
+                    kShooterAngleEncoderOffset = Rotation2d.fromRotations(0); // TODO: Update value
+
                     break;
                 case SIMULATION_BOT:
-                    kShooterKd.initDefault(1);
-                    kShooterKp.initDefault(0.6);
-                    kShooterKff.initDefault(0);
+                    kRollerKff.initDefault(0.017);
+                    kRollerKp.initDefault(0.1);
+                    kRollerKd.initDefault(0.0);
+
+                    kRotationKff.initDefault(0.0);
+                    kRotationKp.initDefault(2.0);
+                    kRotationKd.initDefault(0.0);
+
+                    kShooterAngleEncoderOffset = Rotation2d.fromRotations(Math.random());
+
                     break;
                 default:
                     throw new UnsupportedOperationException("No shooter configuration for " + Configuration.getRobot());
@@ -195,4 +225,20 @@ public static final class Elevator {
         public static final LoggedTunableNumber kElevatorKp = new LoggedTunableNumber("Elevator/Motor/Kp");
         public static final LoggedTunableNumber kElevatorKd = new LoggedTunableNumber("Elevator/Motor/Kd");
     }
+
+    public static final class Indexer {
+        // TODO: Update values and set names
+        public static final int kIndexerMotorId = 0;
+        public static final double kIndexerReduction = 1;
+
+        public static final int kIndexerEntranceSensorId = 1;
+        public static final int kIndexerExitSensorId = 3;
+
+        public static final double kIndexerLoadPercent = .25;
+        public static final double kIndexerFeedPercent = 1;
+
+        public static final LoggedTunableNumber kIndexerKff = new LoggedTunableNumber("Indexer/Motor/Kff");
+        public static final LoggedTunableNumber kIndexerKp = new LoggedTunableNumber("Indexer/Motor/Kp");
+        public static final LoggedTunableNumber kIndexerKd = new LoggedTunableNumber("Indexer/Motor/Kd");
+    }
 }

src/main/java/com/team1701/robot/FieldConstants.java

@@ -9,10 +9,11 @@ public final class FieldConstants {
     public static double kCenterLine = kFieldLongLengthMeters / 2.0;
     public static double kWingLength = Units.inchesToMeters(231.2);
 
+    public static double kSpeakerHeight = 204.5;
     public static Translation3d kBlueSpeakerOpeningCenter =
-            new Translation3d(Units.inchesToMeters(0.9), Units.inchesToMeters(218.42), 204.5);
+            new Translation3d(Units.inchesToMeters(0.9), Units.inchesToMeters(218.42), kSpeakerHeight);
     public static Translation3d kRedSpeakerOpeningCenter = new Translation3d(
             kFieldLongLengthMeters - kBlueSpeakerOpeningCenter.getX(),
             kBlueSpeakerOpeningCenter.getY(),
-            kBlueSpeakerOpeningCenter.getZ());
+            kSpeakerHeight);
 }

src/main/java/com/team1701/robot/Robot.java

@@ -11,6 +11,7 @@
 import edu.wpi.first.hal.AllianceStationID;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import io.javalin.Javalin;
@@ -99,6 +100,9 @@ private void initializeAdvantageKit() {
         // Build robot container
         mRobotContainer = new RobotContainer();
 
+        // Enable command logging
+        SmartDashboard.putData(CommandScheduler.getInstance());
+
         // Launch web server
         Javalin.create(config -> {
                     config.staticFiles.add(
@@ -112,6 +116,7 @@ private void initializeAdvantageKit() {
     @Override
     public void robotPeriodic() {
         CommandScheduler.getInstance().run();
+        mRobotContainer.getRobotState().periodic();
     }
 
     @Override