Adds scoring features

README.md

@@ -5,15 +5,6 @@
 
 > :warning: **WIP**: We are still too early in the season to add proper features/details.
 
-# Getting Started
-
-## Dependencies
-- For formatting support, install emeraldwalk RunOnSave:
-```
-code --install-extension emeraldwalk.RunOnSave
-```
-
-
 # Robot
 
 ## About the Robot

simgui.json

@@ -1,4 +1,11 @@
 {
+  "HALProvider": {
+    "DIO": {
+      "window": {
+        "visible": true
+      }
+    }
+  },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",

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

@@ -8,13 +8,15 @@
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Filesystem;
 import frc.robot.subsystems.drive.CommandSwerveDrivetrain;
 import java.io.IOException;
 import java.util.Collections;
+import java.util.HashMap;
 import java.util.List;
 
 public final class Constants {
@@ -28,8 +30,16 @@ public static enum Mode {
         REPLAY
     }
 
+    public static final class ConversionConstants {
+        public static final double kRadiansPerSecondToRPM = 60.0 / (2.0 * Math.PI);
+    }
+
     public static final class CANDevices {}
 
+    public static final class SensorConstants {
+        public static final int bannerPort = 1; // TODO: Change this
+    }
+
     public static final class DriveConstants {
         public static final double MaxSpeedMetPerSec = 6;
         public static final double MaxAngularRateRadiansPerSec = Math.PI * 2; // 2 PI is one full
@@ -43,6 +53,15 @@ public static final class FieldConstants {
 
         public static final double midfieldLowThresholdM = 5.87;
         public static final double midfieldHighThresholdM = 10.72;
+
+        public static final Translation2d speakerPose =
+                false // TODO: CHANGE THIS URGENT
+                        // DriverStation.getAlliance().get() == DriverStation.Alliance.Red
+                        ? new Translation2d(
+                                Units.inchesToMeters(652.73), Units.inchesToMeters(218.42))
+                        : new Translation2d(
+                                Units.inchesToMeters(-1.5),
+                                Units.inchesToMeters(218.42)); // TODO: Might have to change these
     }
 
     public static final class VisionConstants {
@@ -225,22 +244,80 @@ public static final class TunerConstants {
                         DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight);
     }
 
-    public static final class Scoring {
-        public static final double aimerkP = 1.0;
+    public static final class ScoringConstants {
+        public static final double aimerkP = 6.0;
         public static final double aimerkI = 0.0;
-        public static final double aimerkD = 0.0;
+        public static final double aimerkD = 1.0;
 
         public static final double aimerkS = 0.0;
         public static final double aimerkG = 0.0;
         public static final double aimerkV = 0.0;
         public static final double aimerkA = 0.0;
 
-        public static final double shooterkP = 1.0;
+        public static final double shooterkP = 10.0;
         public static final double shooterkI = 0.0;
         public static final double shooterkD = 0.0;
 
         public static final double shooterkS = 0.0;
         public static final double shooterkV = 0.0;
         public static final double shooterkA = 0.0;
+
+        public static final int aimLeftMotorId = 9;
+        public static final int aimRightMotorId = 10;
+
+        public static final int shooterLeftMotorId = 11;
+        public static final int shooterRightMotorId = 12;
+
+        public static final int aimEncoderPort = 0; // TODO: Change
+
+        public static final int kickerMotorId = 13;
+
+        public static final double aimAcceleration = 1;
+        public static final double aimCruiseVelocity = 1;
+
+        public static final double shooterAcceleration = 1;
+        public static final double shooterJerk = 1;
+
+        public static final double shooterVelocityRPMMargin = 0;
+        public static final double aimAngleRadiansMargin = 0;
+
+        // WARNING - These must by monotonically increasing
+        // Key - Distance in meters
+        // Value - Aimer angle in radians
+        public static HashMap<Double, Double> getAimerMap() { // TODO: Find this
+            HashMap<Double, Double> map = new HashMap<Double, Double>();
+            map.put(0.0, 1.0);
+            map.put(1.0, 0.9);
+            map.put(2.0, 0.85);
+            map.put(3.0, 0.83);
+            map.put(4.0, 0.64);
+            map.put(5.0, 0.59);
+            map.put(6.0, 0.48);
+            map.put(7.0, 0.34);
+            map.put(8.0, 0.27);
+            map.put(9.0, 0.15);
+            map.put(10.0, 0.1);
+
+            return map;
+        }
+
+        // Key - Distance in meters
+        // Value - Shooter RPM
+        public static HashMap<Double, Double> getShooterMap() { // TODO: Find this
+            HashMap<Double, Double> map = new HashMap<Double, Double>();
+            map.put(0.0, 20.0);
+            map.put(1.0, 40.0);
+            map.put(2.0, 60.0);
+            map.put(3.0, 80.0);
+            map.put(4.0, 100.0);
+            map.put(5.0, 120.0);
+            map.put(6.0, 140.0);
+            map.put(7.0, 160.0);
+            map.put(8.0, 180.0);
+            map.put(9.0, 190.0);
+            map.put(10.0, 200.0);
+
+            return map;
+        }
     }
 }

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

@@ -39,7 +39,7 @@ public void robotInit() {
             // Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs") TODO:
             // Add back later
             Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
-            new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
+            pdh = new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
         } else if (Constants.currentMode == Constants.Mode.SIM) {
             // Logger.addDataReceiver(new WPILOGWriter("logs/")); TODO: Add back later
             Logger.addDataReceiver(new NT4Publisher());

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

@@ -16,8 +16,10 @@
 import frc.robot.subsystems.localization.CameraIOPhoton;
 import frc.robot.subsystems.localization.VisionLocalizer;
 import frc.robot.subsystems.scoring.AimerIOSim;
+import frc.robot.subsystems.scoring.AimerIOTalon;
 import frc.robot.subsystems.scoring.ScoringSubsystem;
 import frc.robot.subsystems.scoring.ShooterIOSim;
+import frc.robot.subsystems.scoring.ShooterIOTalon;
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.List;
@@ -41,44 +43,48 @@ public RobotContainer() {
         configureModes();
     }
 
+    // spotless:off
     private void configureBindings() {
-        // spotless:off
-        drivetrain.setDefaultCommand(new DriveWithJoysticks(drivetrain,
-        () -> -controller.getLeftY(),
-        () -> -controller.getLeftX(),
-        () -> -controller.getRightX(),
-        () -> true,
-        () -> false));
-
-        controller.a().onTrue(new InstantCommand(
-                                () -> scoringSubsystem.setAction(
-                                                ScoringSubsystem.ScoringAction.INTAKE)))
+        drivetrain.setDefaultCommand(
+                new DriveWithJoysticks(
+                        drivetrain,
+                        () -> -controller.getLeftY(),
+                        () -> -controller.getLeftX(),
+                        () -> -controller.getRightX(),
+                        () -> true,
+                        () -> false));
+
+        controller.a()
+                .onTrue(new InstantCommand(
+                    () -> scoringSubsystem.setAction(
+                        ScoringSubsystem.ScoringAction.INTAKE)));
+
+        controller.b()
+                .onTrue(new InstantCommand(
+                    () -> scoringSubsystem.setAction(
+                        ScoringSubsystem.ScoringAction.AIM)));
+
+        controller.x()
+                .onTrue(new InstantCommand(
+                    () -> scoringSubsystem.setAction(
+                        ScoringSubsystem.ScoringAction.SHOOT)))
                 .onFalse(new InstantCommand(
-                                () -> scoringSubsystem.setAction(
-                                                ScoringSubsystem.ScoringAction.WAIT)));
+                    () -> scoringSubsystem.setAction(
+                        ScoringSubsystem.ScoringAction.AIM)));
 
-        controller.b().onTrue(new InstantCommand(
-                                () -> scoringSubsystem.setAction(
-                                                ScoringSubsystem.ScoringAction.AIM)))
+        controller.y()
+                .onTrue(new InstantCommand(
+                    () -> scoringSubsystem.setAction(
+                        ScoringSubsystem.ScoringAction.ENDGAME)))
                 .onFalse(new InstantCommand(
-                                () -> scoringSubsystem.setAction(
-                                                ScoringSubsystem.ScoringAction.WAIT)));
+                    () -> scoringSubsystem.setAction(
+                        ScoringSubsystem.ScoringAction.WAIT)));
 
-        controller.x().onTrue(new InstantCommand(
-                                () -> scoringSubsystem.setAction(
-                                                ScoringSubsystem.ScoringAction.SHOOT)))
-                .onFalse(new InstantCommand(
-                                () -> scoringSubsystem.setAction(
-                                                ScoringSubsystem.ScoringAction.WAIT)));
-
-        controller.y().onTrue(new InstantCommand(
-                                () -> scoringSubsystem.setAction(
-                                                ScoringSubsystem.ScoringAction.ENDGAME)))
-                .onFalse(new InstantCommand(
-                                () -> scoringSubsystem.setAction(
-                                                ScoringSubsystem.ScoringAction.WAIT)));
-        // spotless:on
-    }
+        controller.start()
+                .onTrue(new InstantCommand(
+                    () -> scoringSubsystem.setAction(
+                        ScoringSubsystem.ScoringAction.WAIT)));
+    } // spotless:on
 
     private void configureModes() {}
 
@@ -91,7 +97,11 @@ public void configureSubsystems() {
 
         switch (Constants.currentMode) {
             case REAL:
-                scoringSubsystem = new ScoringSubsystem(new ShooterIOSim(), new AimerIOSim());
+                scoringSubsystem =
+                        new ScoringSubsystem(
+                                new ShooterIOTalon(),
+                                new AimerIOTalon(),
+                                driveTelemetry::getFieldToRobot);
 
                 List<CameraIO> realCameras = new ArrayList<>();
                 for (CameraParams params : VisionConstants.cameras) {
@@ -100,7 +110,11 @@ public void configureSubsystems() {
                 tagVision = new VisionLocalizer(realCameras);
                 break;
             case SIM:
-                scoringSubsystem = new ScoringSubsystem(new ShooterIOSim(), new AimerIOSim());
+                scoringSubsystem =
+                        new ScoringSubsystem(
+                                new ShooterIOSim(),
+                                new AimerIOSim(),
+                                driveTelemetry::getFieldToRobot);
 
                 tagVision = new VisionLocalizer(Collections.emptyList());
                 break;

src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java

@@ -3,36 +3,75 @@
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import frc.robot.Constants;
-import frc.robot.Constants.Scoring;
+import frc.robot.Constants.ScoringConstants;
 
 public class AimerIOSim implements AimerIO {
     // TODO: Tune this later
     private final SingleJointedArmSim sim =
-            new SingleJointedArmSim(DCMotor.getNeoVortex(2), 1.0, 1.0, 1.0, 0.0, 1.0, true, 0.0);
+            new SingleJointedArmSim(
+                    DCMotor.getNeoVortex(2),
+                    1.0,
+                    SingleJointedArmSim.estimateMOI(0.5, 4.5),
+                    0.5,
+                    0.0,
+                    2.0,
+                    false,
+                    0.0);
     private final PIDController controller =
-            new PIDController(Scoring.aimerkP, Scoring.aimerkI, Scoring.aimerkD);
+            new PIDController(
+                    ScoringConstants.aimerkP, ScoringConstants.aimerkI, ScoringConstants.aimerkD);
     private final ArmFeedforward feedforward =
-            new ArmFeedforward(Scoring.aimerkS, Scoring.aimerkG, Scoring.aimerkV, Scoring.aimerkA);
+            new ArmFeedforward(
+                    ScoringConstants.aimerkS,
+                    ScoringConstants.aimerkG,
+                    ScoringConstants.aimerkV,
+                    ScoringConstants.aimerkA);
+    private final TrapezoidProfile profile =
+            new TrapezoidProfile(new TrapezoidProfile.Constraints(0.8, 0.4));
 
-    double goalAngRad = 0.0;
+    private final Timer timer = new Timer();
+
+    double goalAngleRad = 0.0;
     double appliedVolts = 0.0;
 
+    double initialAngle = 0.0;
+    double initialVelocity = 0.0;
+
     @Override
     public void setAimAngleRad(double angle) {
-        goalAngRad = angle;
+        if (goalAngleRad != angle) {
+            timer.reset();
+            timer.start();
+
+            initialAngle = sim.getAngleRads();
+            initialVelocity = sim.getVelocityRadPerSec();
+        }
+        goalAngleRad = angle;
     }
 
     @Override
     public void updateInputs(AimerIOInputs inputs) {
         sim.update(Constants.loopTime);
 
+        State trapezoidSetpoint =
+                profile.calculate(
+                        timer.get(),
+                        new State(initialAngle, initialVelocity),
+                        new State(goalAngleRad, 0));
+
         appliedVolts =
-                feedforward.calculate(sim.getAngleRads(), sim.getVelocityRadPerSec())
-                        + controller.calculate(sim.getAngleRads(), goalAngRad);
+                feedforward.calculate(trapezoidSetpoint.position, trapezoidSetpoint.velocity)
+                        + controller.calculate(sim.getAngleRads(), trapezoidSetpoint.position);
+        // appliedVolts = controller.calculate(sim.getAngleRads(), goalAngleRad);
         sim.setInputVoltage(appliedVolts);
 
+        // inputs.aimGoalAngleRad = goalAngleRad;
+        inputs.aimGoalAngleRad = trapezoidSetpoint.position;
         inputs.aimAngleRad = sim.getAngleRads();
 
         inputs.aimAppliedVolts = appliedVolts;