added pivot and shooter

yay

src/main/java/frc/robot/commands/ShooterCalc.java

@@ -0,0 +1,58 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// 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.commands;
+import java.util.Optional;
+import java.util.function.BooleanSupplier;
+
+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.Commands;
+import frc.robot.subsystems.shooter.*;
+import frc.robot.util.Constants.FieldConstants;
+import frc.robot.util.Constants.ShooterConstants;
+import frc.robot.util.SpeedAnglePair;
+
+public class ShooterCalc extends Command {
+
+  public Command prepareShooter(BooleanSupplier supp, Pose2d robotPose, Pivot pivot, Shooter shooter) {
+    SpeedAnglePair pair = (supp.getAsBoolean()) 
+            ? getSpeaker(robotPose) 
+            : getAmp(robotPose);
+    return Commands.runOnce(() -> pivot.setAngle(pair.getAngle()))
+            .alongWith(Commands.runOnce(() -> shooter.setSpeed(pair.getSpeed())));
+  }
+
+  private SpeedAnglePair getSpeaker(Pose2d robotPose) {
+    return calculateSpeed(robotPose, true);
+  }
+
+  private SpeedAnglePair getAmp(Pose2d robotPose) {
+    return calculateSpeed(robotPose, false);
+  }
+
+  private 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;
+
+        // 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 / 5.0) * 5;
+        int upperIndex = (int) Math.ceil(distance / 5.0) * 5;
+
+        SpeedAnglePair lowerPair = ShooterConstants.SPEAKER_DISTANCES_TO_SPEEDS_AND_ANGLE_MAP.get(lowerIndex);
+        SpeedAnglePair upperPair = ShooterConstants.SPEAKER_DISTANCES_TO_SPEEDS_AND_ANGLE_MAP.get(upperIndex);
+
+        return lowerPair.interpolate(upperPair, (distance - lowerIndex) / (upperIndex - lowerIndex));
+    }
+}

src/main/java/frc/robot/subsystems/shooter/Pivot.java

@@ -0,0 +1,40 @@
+package frc.robot.subsystems.shooter;
+
+import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.util.Neo;
+import frc.robot.util.Constants.ShooterConstants;
+
+public class Pivot extends SubsystemBase {
+    private Neo pivot;
+
+    public Pivot() {
+        this.pivot = new Neo(ShooterConstants.SHOOTER_PIVOT_CAN_ID);
+
+        configMotor();
+    }
+
+
+    public void configMotor() {
+        pivot.setSmartCurrentLimit(ShooterConstants.PIVOT_CURRENT_LIMIT);
+        pivot.setPeriodicFramePeriod(PeriodicFrame.kStatus3, 65535);
+        pivot.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 65535);
+        pivot.setInverted(false);
+
+        pivot.setPID(
+                ShooterConstants.PIVOT_P,
+                ShooterConstants.PIVOT_I,
+                ShooterConstants.PIVOT_D,
+                ShooterConstants.PIVOT_MIN_OUTPUT,
+                ShooterConstants.PIVOT_MAX_OUTPUT);
+
+
+        //sets brake mode
+        pivot.setBrakeMode();
+    }
+
+    public void setAngle(double angle) {
+        pivot.setTargetPosition(angle);
+    }
+}

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

@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// 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.shooter;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.util.Neo;
+import frc.robot.util.Constants.ShooterConstants;
+
+public class Shooter extends SubsystemBase {
+    /** Creates a new shooter. */
+    private final Neo motorLeft;
+    private final Neo motorRight;
+
+    public Shooter() {
+
+        motorLeft = new Neo(ShooterConstants.LEFT_SHOOTER_CAN_ID);
+        motorRight = new Neo(ShooterConstants.RIGHT_SHOOTER_CAN_ID);
+
+        configMotors();
+
+    }
+
+    public void configMotors() {
+        motorLeft.setSmartCurrentLimit(ShooterConstants.SHOOTER_CURRENT_LIMIT);
+        motorRight.setSmartCurrentLimit(ShooterConstants.SHOOTER_CURRENT_LIMIT);
+        motorLeft.setPID(
+                ShooterConstants.SHOOTER_P,
+                ShooterConstants.SHOOTER_I,
+                ShooterConstants.SHOOTER_D,
+                ShooterConstants.SHOOTER_MIN_OUTPUT,
+                ShooterConstants.SHOOTER_MAX_OUTPUT);
+
+        motorLeft.addFollower(motorRight, true);
+    }
+
+    @Override
+    public void periodic() {
+        // This method will be called once per scheduler run
+    }
+
+    public void setSpeed(double calc) {
+        motorLeft.setTargetVelocity(calc);
+    }
+
+    public void stop() {
+        motorLeft.setTargetVelocity(0);
+    }
+}

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

@@ -104,9 +104,19 @@ public static final class ShooterConstants {
         public static final double SHOOTER_I = 0;
         public static final double SHOOTER_D = 0;
 
+        public static final double PIVOT_P = 0.01;
+        public static final double PIVOT_I = 0;
+        public static final double PIVOT_D = 0;
+
+        public static final int SHOOTER_CURRENT_LIMIT = 15;
+        public static final int PIVOT_CURRENT_LIMIT = 15; 
+
         // These are in %
         public static final double SHOOTER_MIN_OUTPUT = -1;
         public static final double SHOOTER_MAX_OUTPUT = 1;
+
+        public static final double PIVOT_MIN_OUTPUT = -1;
+        public static final double PIVOT_MAX_OUTPUT = 1;
 
 
         public static final double MEASUREMENT_INTERVAL_FEET = 1.0;