Speed angle pair to main (#34)

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

@@ -32,15 +32,15 @@ public class RobotContainer implements Logged {
     @SuppressWarnings("unused")
     private final DriverUI driverUI;
 
-    private final Climb elevator;
+    private final Climb climb;
 
     public RobotContainer() {
         driver = new PatriBoxController(OIConstants.DRIVER_CONTROLLER_PORT, OIConstants.DRIVER_DEADBAND);
         operator = new PatriBoxController(OIConstants.OPERATOR_CONTROLLER_PORT, OIConstants.OPERATOR_DEADBAND);
         DriverStation.silenceJoystickConnectionWarning(true);
 
         intake = new Intake();
-        elevator = new Climb();
+        climb = new Climb();
         swerve = new Swerve();
         driverUI = new DriverUI();
 
@@ -65,8 +65,8 @@ private void configureButtonBindings(){
     }
 
     private void configureOperatorBindings() {
-        operator.y().onTrue((elevator.toTop(PoseCalculations.getChainPosition(swerve.getPose()))));
-        operator.a().onTrue((elevator.toBottom()));
+        operator.y().onTrue((climb.toTop(PoseCalculations.getChainPosition(swerve.getPose()))));
+        operator.a().onTrue((climb.toBottom()));
     }
 
     private void configureDriverBindings() {

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

@@ -4,52 +4,81 @@
 
 package frc.robot.subsystems;
 
-import com.revrobotics.SparkPIDController;
-
+import java.util.Optional;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.util.Neo;
+import frc.robot.util.SpeedAnglePair;
+import frc.robot.util.Constants.FieldConstants;
 import frc.robot.util.Constants.ShooterConstants;
 
 public class Shooter extends SubsystemBase {
-  /** Creates a new shooter. */
-  private final Neo motorLeft;
-  private final Neo motorRight;
+    /** Creates a new shooter. */
+    private final Neo motorLeft;
+    private final Neo motorRight;
+    private final Neo pivotMotor;
+
+    public Shooter() {
+
+        motorLeft = new Neo(ShooterConstants.LEFT_SHOOTER_CAN_ID);
+        motorRight = new Neo(ShooterConstants.RIGHT_SHOOTER_CAN_ID);
+
+        pivotMotor = new Neo(ShooterConstants.SHOOTER_PIVOT_CAN_ID);
+
+        configMotors();
+
+    }
+
+    public void configMotors() {
+        motorLeft.setPID(
+                ShooterConstants.SHOOTER_P,
+                ShooterConstants.SHOOTER_I,
+                ShooterConstants.SHOOTER_D,
+                ShooterConstants.SHOOTER_MIN_OUTPUT,
+                ShooterConstants.SHOOTER_MAX_OUTPUT);
 
-  public Shooter() {
+        motorLeft.addFollower(motorRight, true);
+        // Current limit
+        // Velocity conversion factor (make gear ratio in constants)
 
-    motorLeft = new Neo(ShooterConstants.LEFT_SHOOTER_CAN_ID);
-    motorRight = new Neo(ShooterConstants.RIGHT_SHOOTER_CAN_ID);
+    }
 
-    configMotors();
+    @Override
+    public void periodic() {
+        // This method will be called once per scheduler run
+    }
 
-  }
+    public Command shoot(Pose2d position, boolean shootingAtSpeaker) {
+        SpeedAnglePair pair = calculateSpeed(position, shootingAtSpeaker);
+        return runOnce(() -> motorLeft.setTargetVelocity(1));
+    }
 
-  public void configMotors() {
-      motorLeft.setPID(
-        ShooterConstants.SHOOTER_P,
-        ShooterConstants.SHOOTER_I,
-        ShooterConstants.SHOOTER_D,
-        ShooterConstants.SHOOTER_MIN_OUTPUT,
-        ShooterConstants.SHOOTER_MAX_OUTPUT
-      );
+    public Command stop() {
+        return runOnce(() -> motorLeft.setTargetVelocity(0));
+    }
 
-      motorLeft.addFollower(motorRight, true);
-      // Current limit
-      // Soft stops (dont extend too high)
-      // Position conversion factor (make gear ratio in constants)
-      // 
-  }
+    public SpeedAnglePair calculateSpeed(Pose2d robotPose, boolean shootingAtSpeaker) {
+        // Constants have blue alliance positions at index 0
+        // and red alliance positions at index 1
+        int positionIndex = FieldConstants.ALLIANCE == Optional.ofNullable(Alliance.Blue) ? 0 : 1;
 
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-  }
+        // Get our position relative to the desired field element
+        if (shootingAtSpeaker) {
+            robotPose.relativeTo(FieldConstants.SPEAKER_POSITIONS[positionIndex]);
+        } else {
+            robotPose.relativeTo(FieldConstants.AMP_POSITIONS[positionIndex]);
+        }
 
+        double distance = Units.metersToFeet(robotPose.getTranslation().getNorm());
+        int lowerIndex = (int) (Math.floor(distance / ShooterConstants.MEASUREMENT_INTERVAL_FEET) * ShooterConstants.MEASUREMENT_INTERVAL_FEET);
+        int upperIndex = (int) (Math.ceil(distance / ShooterConstants.MEASUREMENT_INTERVAL_FEET) * ShooterConstants.MEASUREMENT_INTERVAL_FEET);
+
+        SpeedAnglePair lowerPair = ShooterConstants.SPEAKER_DISTANCES_TO_SPEEDS_AND_ANGLE_MAP.get(lowerIndex);
+        SpeedAnglePair upperPair = ShooterConstants.SPEAKER_DISTANCES_TO_SPEEDS_AND_ANGLE_MAP.get(upperIndex);
 
-  public Command shoot(Pose2d distanceToTarget, boolean shootingAtSpeaker) {
-
-    return runOnce(() -> motorLeft.setTargetVelocity(1));
-  }
+        return lowerPair.interpolate(upperPair, (distance - lowerIndex) / (upperIndex - lowerIndex));
+    }
 }

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

@@ -30,6 +30,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.DriverUI;
 import frc.robot.commands.Drive;
+import frc.robot.util.MAXSwerveModule;
 import frc.robot.util.Constants.AutoConstants;
 import frc.robot.util.Constants.DriveConstants;
 import frc.robot.util.Constants.FieldConstants;

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

@@ -5,12 +5,15 @@
 package frc.robot.util;
 
 import java.util.ArrayList;
+import java.util.HashMap;
 
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
 import com.pathplanner.lib.util.ReplanningConfig;
 import com.revrobotics.CANSparkBase;
 import java.util.Optional;
+
+import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.controller.HolonomicDriveController;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
@@ -105,6 +108,18 @@ public static final class ShooterConstants {
         public static final double SHOOTER_MIN_OUTPUT = -1;
         public static final double SHOOTER_MAX_OUTPUT = 1;
 
+
+        public static final double MEASUREMENT_INTERVAL_FEET = 1.0;
+        /**
+         * The distances are in feet, the speeds are in RPM, and the angles are in degrees.
+         */
+        public static final HashMap<Integer, SpeedAnglePair> SPEAKER_DISTANCES_TO_SPEEDS_AND_ANGLE_MAP = new HashMap<Integer, SpeedAnglePair>() {{
+            put(5,  SpeedAnglePair.of(0.4, 10.0));
+            put(10, SpeedAnglePair.of(0.4, 10.0));
+            put(15, SpeedAnglePair.of(0.4, 10.0));
+            put(20, SpeedAnglePair.of(0.4, 10.0));
+            put(25, SpeedAnglePair.of(0.4, 10.0));
+        }};
     }
 
     public static final class TrapConstants {
@@ -275,6 +290,7 @@ public static final class FieldConstants {
 
         public static final double ALLOWABLE_ERROR_METERS = Units.inchesToMeters(2);
         public static final double FIELD_WIDTH_METERS = 16.53;
+        public static final double FIELD_HEIGHT_METERS = 8.278;
 
         public static Optional<Alliance> ALLIANCE = Optional.empty();
 
@@ -288,7 +304,7 @@ public static enum GameMode {
         public static GameMode GAME_MODE;
 
         // Field: https://cad.onshape.com/documents/dcbe49ce579f6342435bc298/w/b93673f5b2ec9c9bdcfec487/e/6ecb2d6b7590f4d1c820d5e3
-        // Chain Positions:
+        // Chain Positions: Blue alliance left
         // @formatter:off
         //  2              5
         //      1      4
@@ -304,7 +320,32 @@ public static enum GameMode {
                 // Red Stage
                 new Pose2d(12.07, 3.237, Rotation2d.fromDegrees(120)),
                 new Pose2d(10.638, 4.204, Rotation2d.fromDegrees(0)),
-                new Pose2d(12.2, 5, Rotation2d.fromDegrees(-120))
+        new Pose2d(12.2, 5, Rotation2d.fromDegrees(-120))
+        };
+
+        // Speaker Positions: Blue alliance left
+        // @formatter:off
+        //
+        //  0             1
+        //
+        //
+        // @formatter:on
+        public static Pose2d[] SPEAKER_POSITIONS = new Pose2d[] {
+                // All points are in meters and radians
+                // All relative to the blue origin
+                // Blue Speaker
+                new Pose2d(0, 5.547, Rotation2d.fromDegrees(0)),
+                // Red Speaker
+                new Pose2d(FIELD_WIDTH_METERS, 5.547, Rotation2d.fromDegrees(180)),
+        };
+
+        public static Pose2d[] AMP_POSITIONS = new Pose2d[] {
+                // All points are in meters and radians
+                // All relative to the blue origin
+                // Blue Speaker
+                new Pose2d(1.827, FIELD_HEIGHT_METERS, Rotation2d.fromDegrees(-90)),
+                // Red Speaker
+                new Pose2d(14.706, FIELD_HEIGHT_METERS, Rotation2d.fromDegrees(-90)),
         };
 
         public static final double CHAIN_LENGTH_METERS = Units.inchesToMeters(100);

src/main/java/frc/robot/subsystems/MAXSwerveModule.java → src/main/java/frc/robot/util/MAXSwerveModule.java

@@ -2,7 +2,7 @@
 // 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;
+package frc.robot.util;
 
 import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.CANSparkBase;
@@ -14,9 +14,6 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.util.Neo;
 import frc.robot.util.Constants.FieldConstants;
 import frc.robot.util.Constants.ModuleConstants;
 

src/main/java/frc/robot/util/SpeedAnglePair.java

@@ -0,0 +1,31 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Pair;
+
+public class SpeedAnglePair extends Pair<Double, Double> {
+
+    public SpeedAnglePair(Double speed, Double angle) {
+        super(speed, angle);
+    }
+
+    public Double getSpeed() {
+        return super.getFirst();
+    }
+
+    public Double getAngle() {
+        return super.getSecond();
+    }
+
+    public static SpeedAnglePair of(Double speed, Double angle) {
+        return new SpeedAnglePair(speed, angle);
+    }
+
+    public SpeedAnglePair interpolate(SpeedAnglePair other, double t) {
+        return SpeedAnglePair.of(
+            MathUtil.interpolate(this.getSpeed(), other.getSpeed(), t),
+            MathUtil.interpolate(this.getAngle(), other.getAngle(), t)
+        );
+    }
+
+}