Merge branch 'origin/swerve' into robot-logger

src/main/java/frc4388/robot/Constants.java

@@ -4,12 +4,15 @@
 
 package frc4388.robot;
 
+import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
+
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+
 import frc4388.utility.Gains;
 import frc4388.utility.LEDPatterns;
 
@@ -52,11 +55,12 @@ public final class Constants {
     public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
     public static final int GYRO_ID = 14;
     
-    // ofsets are in degrees
-    public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
-    public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
-    public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0;
-    public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0;
+    // offsets are in degrees
+    // NATHAN if you truncate or round or simplify these i will cry
+    public static final double LEFT_FRONT_ENCODER_OFFSET = 181.45-3.30;//181.7578125;//180.0;//315.0 +45;//180.0;
+    public static final double RIGHT_FRONT_ENCODER_OFFSET = 360.-59.0625+0.18;//360.-59.0625;//315.0;//224.296875 + 90;//300.0;
+    public static final double LEFT_BACK_ENCODER_OFFSET = 360.-128.222;//308.408203125;//225.0;//45.87890625;//360.0 - 128.0;
+    public static final double RIGHT_BACK_ENCODER_OFFSET = 360.+2.15-3.637;//180-2.021484375;//0.0;//134.384765625 + 45;
 
     // swerve PID constants
     public static final int SWERVE_SLOT_IDX = 0;
@@ -67,10 +71,10 @@ public final class Constants {
     // swerve auto constants
     public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0);
     public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0);
-    public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI));
+    public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController (15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI));
     public static final boolean PATH_RECORD_VELOCITY = true;
-    public static final double PATH_MAX_VELOCITY = 5.5;
-    public static final double PATH_MAX_ACCELERATION = 50;
+    public static final double PATH_MAX_VELOCITY = 5.0;
+    public static final double PATH_MAX_ACCELERATION = 5.0;
     public static final double MIN_WAYPOINT_ANGLE = 20;
     public static final double MIN_WAYPOINT_DISTANCE = 0.1;
     public static final double MIN_WAYPOINT_VELOCITY = 0.1;
@@ -81,10 +85,11 @@ public final class Constants {
     public static final int REMOTE_0 = 0;
 
     // conversions
-    // gear ratio: 5.12 rev motor = 1 rev wheel
+    // gear ratio: 5.14 rev motor = 1 rev wheel
     // wheel diameter: official = 4 in, measured = 3.8 in
     /* Ratio Calculation */
-    public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
+    public static final double MOTOR_REV_PER_STEER_REV = 12.8;
+    public static final double MOTOR_REV_PER_WHEEL_REV = 5.142857;
     public static final double WHEEL_DIAMETER_INCHES = 4.0;
     public static final double TICKS_PER_MOTOR_REV = 2048;
     public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
@@ -119,4 +124,62 @@ public final class Constants {
     public static final double LEFT_AXIS_DEADBAND = 0.1;
     public static final double RIGHT_AXIS_DEADBAND = 0.6;
   }
+
+  public static final class ShooterConstants {
+    /* PID Constants Shooter */
+    public static final int CLOSED_LOOP_TIME_MS = 1;
+
+    public static final int SHOOTER_TIMEOUT_MS = 32;
+    public static final int SHOOTER_SLOT_IDX = 0;
+    public static final int SHOOTER_PID_LOOP_IDX = 1;
+    public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5);
+    public static final int SHOOTER_FALCON_LEFT_CAN_ID = 23;
+    public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 24; 
+    public static final int SHOOTER_ROTATE_ID = 31; // TODO: find
+    public static final double TURRET_SPEED_MULTIPLIER = 0.1d;
+    public static final int DEGREES_PER_ROT = 0;
+    public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0;
+    public static final int TURRET_MOTOR_ROTS_PER_ROT = 0;
+    public static final double ENCODER_TICKS_PER_REV = 2048;
+    
+    
+   
+    /* Turret Constants */
+    //ID
+    public static final int TURRET_MOTOR_CAN_ID = 30;
+    public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0);
+    public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
+    public static final double SHOOTER_TURRET_MIN = -1.0;
+    public static final float TURRET_FORWARD_LIMIT = 0; // TODO: find
+    public static final float TURRET_REVERSE_LIMIT = 0; // TODO: find
+
+    // deadzones
+    public static final double HARD_DEADZONE_LEFT = 0.0;
+    public static final double HARD_DEADZONE_RIGHT = 340.0;
+
+    public static final double DIG_DEADZONE_LEFT = 40.0;
+    public static final double DIG_DEADZONE_RIGHT = 60.0;
+
+    public static final int SHOOTER_FALCON_BALLER_ID = 0; // TODO: find
+    public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"//
+  
+    public static final Gains DRUM_SHOOTER_GAINS = new Gains(0,0,0,0,0,0); // TODO: tune values
+
+    /* Hood Constants */
+    public static final int SHOOTER_ANGLE_ADJUST_ID = 32;
+    public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find
+    public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find
+    public static final float HOOD_FORWARD_LIMIT = 0; //TODO: find
+    public static final float HOOD_REVERSE_LIMIT = 0; //TODO: find
+
+
+  }
+  public static final class VisionConstants {
+    public static final double TURN_P_VALUE = 0.8;
+    public static final double X_ANGLE_ERROR = 0.5;
+    public static final double GRAV = 385.83;
+    public static final double TARGET_HEIGHT = 67.5;
+    public static final double FOV = 29.8; //Field of view limelight
+    public static final double LIME_ANGLE = 24.7;
+  }
 }

src/main/java/frc4388/robot/Main.java

@@ -35,5 +35,4 @@ public final class Main {
   }
 }
 
-
-// hi ryan
+// hi ryan -aarav

src/main/java/frc4388/robot/Robot.java

@@ -105,6 +105,8 @@ public class Robot extends TimedRobot {
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
+
+    // print odometry data to smart dashboard for debugging (if causing timeout errors, you can comment it)
     SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX());
     SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY());
     SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
@@ -132,9 +134,6 @@ public class Robot extends TimedRobot {
 
   @Override
   public void disabledPeriodic() {
-    // SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX());
-    // SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY());
-    // SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
   }
 
   /**
@@ -162,6 +161,7 @@ public class Robot extends TimedRobot {
   @Override
   public void teleopInit() {
     LOGGER.fine("teleopInit()");
+    m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw());
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
@@ -178,8 +178,6 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void teleopPeriodic() {
-    // m_robotContainer.getDriverController().updateInput();
-    // m_robotContainer.getOperatorController().updateInput();
   }
 
   @Override

src/main/java/frc4388/robot/RobotContainer.java

@@ -53,8 +53,13 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.robot.commands.AimToCenter;
+import frc4388.robot.subsystems.BoomBoom;
+import frc4388.robot.subsystems.Hood;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.Turret;
+import frc4388.robot.subsystems.Vision;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.ListeningSendableChooser;
 import frc4388.utility.PathPlannerUtil;
@@ -74,11 +79,13 @@ public class RobotContainer {
   private final RobotMap m_robotMap = new RobotMap();
 
   /* Subsystems */
-  private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
-      m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
+  public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
 
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
-
+  private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
+  private final Hood m_robotHood = new Hood();
+  private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
+  private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom);
   /* Controllers */
   private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
   private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -99,7 +106,14 @@ public class RobotContainer {
   public RobotContainer() {
     configureButtonBindings();
     /* Default Commands */
-    // drives the swerve drive with a two-axis input from the driver controller
+    
+    // continually sends updates to the Blinkin LED controller to keep the lights on
+    // m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
+
+    //Turret default command
+
+    m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive));
+
     m_robotSwerveDrive.setDefaultCommand(
         new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
             getDriverController().getLeftX(),
@@ -123,9 +137,9 @@ public class RobotContainer {
    */
   private void configureButtonBindings() {
     /* Driver Buttons */
-    new JoystickButton(getDriverController(), XboxController.Button.kY.value)
-        // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.Y_BUTTON)
-        .whenPressed(m_robotSwerveDrive.m_gyro::reset);
+    // "XboxController.Button.kBack" was undefined yet, 7 works just fine
+    new JoystickButton(getDriverController(), 7)
+      .whenPressed(m_robotSwerveDrive::resetGyro);
 
     new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
         // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
@@ -141,9 +155,16 @@ public class RobotContainer {
     /* Operator Buttons */
     // activates "Lit Mode"
     new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
-        // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.A_BUTTON)
         .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
         .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
+    // activates "BoomBoom"
+    new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
+        .whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1))
+        .whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0));
+    // activates hood
+    new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
+        .whenPressed(() -> m_robotHood.runHood(0.5d))
+        .whenReleased(() -> m_robotHood.runHood(0.d));
   }
 
   /**
@@ -177,10 +198,18 @@ public class RobotContainer {
     return m_driverXbox;
   }
 
+  /**
+   * Get odometry.
+   * @return Odometry
+   */
   public Pose2d getOdometry() {
     return m_robotSwerveDrive.getOdometry();
   }
 
+  /**
+   * Set odometry to given pose.
+   * @param pose Pose to set odometry to.
+   */
   public void resetOdometry(Pose2d pose) {
     m_robotSwerveDrive.resetOdometry(pose);
   }

src/main/java/frc4388/robot/RobotMap.java

@@ -5,13 +5,20 @@
 package frc4388.robot;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
+import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
+import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.WPI_PigeonIMU;
 
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 
 import frc4388.robot.Constants.LEDConstants;
+import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.subsystems.SwerveModule;
 
@@ -24,16 +31,18 @@ public class RobotMap {
   public RobotMap() {
     configureLEDMotorControllers();
     configureSwerveMotorControllers();
+    configureShooterMotorControllers();
   }
 
   /* LED Subsystem */
   public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
 
   void configureLEDMotorControllers() {
-    
+
   }
-  
+
   /* Swerve Subsystem */
+
   public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
   public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
   public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
@@ -46,6 +55,7 @@ public class RobotMap {
   public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
   public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
   public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
+
   public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID);
 
   public SwerveModule leftFront;
@@ -91,14 +101,15 @@ public class RobotMap {
     rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
 
-    leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
-    leftFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
-    rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
-    rightFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
-    leftBackSteerMotor.setNeutralMode(NeutralMode.Brake);
-    leftBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
-    rightBackSteerMotor.setNeutralMode(NeutralMode.Brake);
-    rightBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
+    NeutralMode mode = NeutralMode.Coast;
+    leftFrontSteerMotor.setNeutralMode(mode);
+    leftFrontWheelMotor.setNeutralMode(mode);//Coast
+    rightFrontSteerMotor.setNeutralMode(mode);
+    rightFrontWheelMotor.setNeutralMode(mode);//Coast
+    leftBackSteerMotor.setNeutralMode(mode);
+    leftBackWheelMotor.setNeutralMode(mode);//Coast
+    rightBackSteerMotor.setNeutralMode(mode);
+    rightBackWheelMotor.setNeutralMode(mode);//Coast
 
     leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
     leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
@@ -106,10 +117,56 @@ public class RobotMap {
     rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
 
     // config cancoder as remote encoder for swerve steer motors
-    //leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    //leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    //rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(),
+    RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(),
+    RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(),
+    RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(),
+    RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+  }
+
+  // Shooter Config
+  /* Boom Boom Subsystem */
+  public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
+  public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
+
+  public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
+
+  // Create motor CANSparkMax
+  void configureShooterMotorControllers() {
+
+    // LEFT FALCON
+    shooterFalconLeft.configFactoryDefault();
+    shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
+    shooterFalconLeft.setInverted(true);
+    shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
+
+    // RIGHT FALCON
+    shooterFalconRight.setInverted(false);
+    shooterFalconRight.setNeutralMode(NeutralMode.Coast);
+    shooterFalconRight.configFactoryDefault();
+    shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    // m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary)
+    shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    
+    /* Turret Subsytem */
+    shooterFalconRight.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers out of our ass anymore
+    shooterFalconLeft.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull numbers out of our ass anymore
+
   }
 
 }

src/main/java/frc4388/robot/commands/AimToCenter.java

@@ -0,0 +1,78 @@
+// 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 frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.ShooterConstants;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.Turret;
+
+public class AimToCenter extends CommandBase {
+  /** Creates a new AimWithOdometry. */
+  Turret m_turret;
+  SwerveDrive m_drive;
+
+  // use odometry to find x and y later
+  double x;
+  double y;
+  double m_targetAngle;
+
+  // public static Gains m_aimGains;
+
+  public AimToCenter(Turret turret, SwerveDrive drive) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_turret = turret;
+    m_drive = drive;
+    addRequirements(m_turret, m_drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    x = 0;
+    y = 0;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
+    m_turret.runshooterRotatePID(m_targetAngle);
+  }
+
+  public static double angleToCenter(double x, double y, double gyro) {
+    double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0)
+    return angle;
+  }
+
+  /**
+   * Checks if in hardware deadzone (due to mechanical limitations).
+   * @param angle Angle to check.
+   * @return True if in hardware deadzone.
+   */
+  public static boolean isHardwareDeadzone(double angle) {
+    return ((ShooterConstants.HARD_DEADZONE_LEFT > angle) || (angle > ShooterConstants.HARD_DEADZONE_RIGHT));
+  }
+
+  /**
+   * Checks if in digital deadzone (due to climber).
+   * @param angle Angle to check.
+   * @return True if in digital deadzone.
+   */
+  public static boolean isDigitalDeadzone(double angle) {
+    return ((ShooterConstants.DIG_DEADZONE_LEFT < angle) && (angle < ShooterConstants.DIG_DEADZONE_RIGHT));
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/Shoot.java

@@ -0,0 +1,157 @@
+// 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 frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import frc4388.robot.subsystems.BoomBoom;
+import frc4388.robot.subsystems.Hood;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.Turret;
+
+public class Shoot extends CommandBase {
+
+  // subsystems
+  public SwerveDrive m_swerve;
+  public BoomBoom m_boomBoom;
+  public Turret m_turret;
+  public Hood m_hood;
+
+  // given
+  public double m_gyroAngle;
+  public double m_odoX;
+  public double m_odoY;
+  public double m_distance;
+
+  // targets
+  public double m_targetVel;
+  public double m_targetHood;
+  public double m_targetAngle;
+  public double m_driveTargetAngle;
+
+  // pid
+  public double error;
+  public double prevError;
+  public double kP, kI, kD;
+  public double proportional, integral, derivative;
+  public double time;
+  public double output;
+  public double tolerance = 5.0;
+
+  // // dummy motor
+  // public WPI_TalonFX dummy = new WPI_TalonFX(69 - 420);
+  // public TalonFXConfiguration dummyConfiguration = new TalonFXConfiguration();
+
+  /** Creates a new Shoot. */
+  public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_swerve = sDrive;
+    m_boomBoom = sShooter;
+    m_turret = sTurret;
+    m_hood = sHood;
+    
+    addRequirements(m_swerve, m_boomBoom, m_turret, m_hood);
+
+    kP = 0.1;
+    kI = 0.0;
+    kD = 0.0;
+
+    proportional = 0;
+    integral = 0;
+    derivative = 0;
+    time = 0.02;
+  }
+
+  /**
+   * Updates error for custom PID.
+   */
+  public void updateError() {
+    error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_odoX = 0; //TODO: get this value using odometry
+    m_odoY = 0; //TODO: get this value using odometry
+    m_distance = Math.sqrt(Math.pow(m_odoX, 2) + Math.pow(m_odoY, 2));
+
+    m_gyroAngle = m_swerve.getRegGyro().getDegrees();
+
+    // get targets (shooter tables)
+    m_targetVel = m_boomBoom.getVelocity(m_distance);
+    m_targetHood = m_boomBoom.getHood(m_distance);
+    m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.;
+    m_driveTargetAngle = m_targetAngle + m_turret.getBoomBoomAngleDegrees();
+
+    // // normal (i think) PID stuff
+    // dummyConfiguration.primaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.IntegratedSensor.toFeedbackDevice();
+    // dummyConfiguration.remoteFilter0.remoteSensorDeviceID = dummy.getDeviceID();
+    // dummyConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor;
+
+    // dummyConfiguration.slot0.kP = 0.1;
+    // dummyConfiguration.slot0.kI = 0;
+    // dummyConfiguration.slot0.kD = 0;
+    // dummyConfiguration.slot0.kF = 0;
+
+    // // weird PID stuff 
+    // dummyConfiguration.auxiliaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.SoftwareEmulatedSensor.toFeedbackDevice();
+    // dummyConfiguration.remoteFilter1.remoteSensorDeviceID = ShooterConstants.TURRET_MOTOR_CAN_ID;
+    // dummyConfiguration.remoteFilter1.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor;
+    // // dummyConfiguration.auxiliaryPID.selectedFeedbackCoefficient = 0;
+  
+    // dummyConfiguration.slot1.kP = 0.1;
+    // dummyConfiguration.slot1.kI = 0;
+    // dummyConfiguration.slot1.kD = 0;
+    // dummyConfiguration.slot1.kF = 0;
+
+    // dummy.configAllSettings(dummyConfiguration);
+
+    // initial error
+    updateError();
+    prevError = error;
+  }
+
+  /**
+   * Run custom PID.
+   */
+  public void runPID() {
+    prevError = error;
+    updateError();
+    
+    proportional = error;
+    integral = integral + error * time;
+    derivative = (error - prevError) / time;
+    output = kP * proportional + kI * integral + kD * derivative;
+  }
+  
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    // dummy.selectProfileSlot(0, 0);
+    // dummy.selectProfileSlot(1, 1);
+    // dummy.set(TalonFXControlMode.Position, m_driveTargetAngle, DemandType.AuxPID, m_targetAngle);
+    // m_swerve.driveWithInput(0, 0, m_driveTargetAngle, true);
+    // m_swerve.driveWithInput(0, 0, Math.cos(m_driveTargetAngle), Math.sin(m_driveTargetAngle), true); // only works for new DWI in swerve branch
+
+    // custom pid
+    runPID();
+    m_swerve.driveWithInput(0, 0, output, true);
+    
+    m_hood.runAngleAdjustPID(m_targetHood);
+    m_boomBoom.runDrumShooterVelocityPID(m_targetVel);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    updateError();
+    return Math.abs(error) <= tolerance;
+  }
+}

src/main/java/frc4388/robot/subsystems/BoomBoom.java

@@ -0,0 +1,142 @@
+// 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 frc4388.robot.subsystems;
+
+import java.io.File;
+import java.io.IOException;
+import java.util.Comparator;
+import java.util.Map;
+import java.util.Optional;
+import java.util.function.Function;
+import java.util.regex.Pattern;
+import java.util.stream.IntStream;
+
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.ShooterConstants;
+import frc4388.utility.CSV;
+import frc4388.utility.Gains;
+
+public class BoomBoom extends SubsystemBase {
+  public WPI_TalonFX m_shooterFalconLeft;
+  public WPI_TalonFX m_shooterFalconRight;
+  public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
+  public static BoomBoom m_boomBoom;
+
+  double velP;
+  double input;
+
+  public boolean m_isDrumReady = false;
+  public double m_fireVel;
+
+  public Hood m_hoodSubsystem;
+  public Turret m_turretSubsystem;
+
+  // SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values
+  // later
+
+  public static class ShooterTableEntry {
+    public Double distance, hoodExt, drumVelocity;
+  }
+
+  private ShooterTableEntry[] m_shooterTable;
+
+  /*
+  * Creates new BoomBoom subsystem, has drum shooter and angle adjuster
+  */
+  /** Creates a new BoomBoom. */
+  public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
+    m_shooterFalconLeft = shooterFalconLeft;
+    m_shooterFalconRight = shooterFalconRight;
+
+    try {
+      CSV<ShooterTableEntry> csv = new CSV<>(ShooterTableEntry::new) {
+        private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");
+
+        @Override
+        protected String headerSanitizer(final String header) {
+          return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
+        }
+
+      };
+      m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances.csv").toPath());
+      new Thread(() -> System.out.println(CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start();
+    } catch (final IOException e) {
+      e.printStackTrace();
+      // throw new RuntimeException(e);
+    }
+  }
+
+  public Double getVelocity(final Double distance) {
+    return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
+  }
+
+  public Double getHood(final Double distance) {
+    return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
+  }
+
+  private static <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
+    final Map.Entry<Integer, E> closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1]));
+    final E closestRecord = closestEntry.getValue();
+    final int closestRecordIndex = closestEntry.getKey();
+    final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))];
+    return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord));
+  }
+
+  private static <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
+    final Optional<Map.Entry<Integer, E>> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue())));
+    return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty();
+  }
+
+  private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) {
+    final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue());
+    return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue();
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+
+  }
+
+  public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) {
+    m_hoodSubsystem = subsystem0;
+    m_turretSubsystem = subsystem1;
+  }
+
+  /**
+   * Runs the Drum motor at a given speed
+   * @param speed percent output form -1.0 to 1.0
+   */
+  public void runDrumShooter(double speed) {
+    m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);
+
+  }
+
+  public void setShooterGains() {
+    m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
+    m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
+  }
+
+  public void runDrumShooterVelocityPID(double targetVel) {
+    m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init
+    m_shooterFalconRight.follow(m_shooterFalconLeft);
+    // New BoomBoom controller stuff
+    // Controls a motor with the output of the BangBang controller
+    // Controls a motor with the output of the BangBang conroller and a feedforward
+    // Shrinks the feedforward slightly to avoid over speeding the shooter
+
+    // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 *
+    // feedforward.calculate(targetVel));
+    // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel));
+  }
+}

src/main/java/frc4388/robot/subsystems/Hood.java

@@ -0,0 +1,96 @@
+// 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 frc4388.robot.subsystems;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxPIDController;
+import com.revrobotics.SparkMaxLimitSwitch;
+import com.revrobotics.CANSparkMax.ControlType;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMax.SoftLimitDirection;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import frc4388.robot.Constants.ShooterConstants;
+import frc4388.utility.Gains;
+
+public class Hood extends SubsystemBase {
+  public BoomBoom m_shooterSubsystem;
+  
+  public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
+  public SparkMaxLimitSwitch m_hoodUpLimitSwitch;
+  public SparkMaxLimitSwitch m_hoodDownLimitSwitch;
+  public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
+  public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder();
+
+  public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController();
+  
+  
+  //public boolean m_isHoodReady = false;
+
+public double m_fireAngle;
+  
+
+  /** Creates a new Hood. */
+  public Hood() {
+     m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
+
+    // m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    // m_hoodUpLimitSwitch.enableLimitSwitch(true);
+    m_hoodDownLimitSwitch.enableLimitSwitch(true);
+
+    m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT);
+    m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT);
+    setHoodSoftLimits(true);
+  }
+    
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+
+    /**
+   * Set status of hood motor soft limits.
+   * @param set Boolean to set soft limits to.
+   */
+  public void setHoodSoftLimits(boolean set) {
+    m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kForward, set);
+    m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kReverse, set);
+  }
+
+  public void runAngleAdjustPID(double targetAngle) 
+  {
+    //Set PID Coefficients
+    m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP);
+    m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI);
+    m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD);
+    m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone);
+    m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF);
+    m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput);
+
+    m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition);
+  }
+  
+
+  public void runHood(double input) {
+    // m_angleAdjusterMotor.set(input);
+  }
+
+  public void resetGyroAngleAdj(){
+    // m_angleEncoder.setPosition(0);
+  }
+
+  public double getAnglePosition(){
+    return 0.0;//m_angleEncoder.getPosition();
+  }
+
+  public double getAnglePositionDegrees(){
+    return 0.0;//((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90;
+  }
+}

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -54,37 +54,19 @@ public class SwerveDrive extends SubsystemBase {
 
   public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
   public boolean ignoreAngles;
-  public Rotation2d rotTarget = new Rotation2d();;
+  public Rotation2d rotTarget = new Rotation2d();
   public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
   private final Field2d m_field = new Field2d();
 
   public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, WPI_PigeonIMU gyro) {
-      // m_leftFrontSteerMotor = leftFrontSteerMotor;
-      // m_leftFrontWheelMotor = leftFrontWheelMotor;
-      // m_rightFrontSteerMotor = rightFrontSteerMotor;
-      // m_rightFrontWheelMotor = rightFrontWheelMotor;
-      // m_leftBackSteerMotor = leftBackSteerMotor;
-      // m_leftBackWheelMotor = leftBackWheelMotor;
-      // m_rightBackSteerMotor = rightBackSteerMotor;
-      // m_rightBackWheelMotor = rightBackWheelMotor;
-      // m_leftFrontEncoder = leftFrontEncoder; 
-      // m_rightFrontEncoder = rightFrontEncoder;
-      // m_leftBackEncoder = leftBackEncoder;
-      // m_rightBackEncoder = rightBackEncoder; 
+
     m_leftFront = leftFront;
     m_leftBack = leftBack;
     m_rightFront = rightFront;
     m_rightBack = rightBack;
     m_gyro = gyro;
 
-      // modules = new SwerveModule[] {
-      //   new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left
-      //   new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET), // Front Right
-      //   new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left
-      //   new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET)  // Back Right
-      // };
-
     modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack};
 
     m_poseEstimator =
@@ -154,21 +136,26 @@ public class SwerveDrive extends SubsystemBase {
       SwerveModuleState state = desiredStates[i];
       module.setDesiredState(state, false);
     }
+    // modules[0].setDesiredState(desiredStates[0], false);
   }
 
   @Override
   public void periodic() {
-    updateOdometry();
-    SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
-    SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
-    SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());
-    SmartDashboard.putNumber("Pigeon Rotation 2D", m_gyro.getRotation2d().getDegrees());
-    SmartDashboard.putStringArray("Fusion Status", new String[] {"Is Fusing: "+fstatus.bIsFusing, "Is Valid: "+fstatus.bIsValid, "Heading: "+fstatus.heading});
 
-    // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
+    updateOdometry();
+    // SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
+
+    SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); 
+
+    // SmartDashboard.putNumber("Front Left", modules[0].driveMotor.getSelectedSensorPosition());
+    // SmartDashboard.putNumber("Front Right", modules[1].driveMotor.getSelectedSensorPosition());
+    // SmartDashboard.putNumber("Back Left", modules[2].driveMotor.getSelectedSensorPosition());
+    // SmartDashboard.putNumber("Back Right", modules[3].driveMotor.getSelectedSensorPosition());
+    // SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());
+    // SmartDashboard.putNumber("Pigeon Rotation 2D", m_gyro.getRotation2d().getDegrees());
+    // SmartDashboard.putStringArray("Fusion Status", new String[] {"Is Fusing: "+fstatus.bIsFusing, "Is Valid: "+fstatus.bIsValid, "Heading: "+fstatus.heading});
+
+    // m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
     super.periodic();
   }
 
@@ -208,27 +195,29 @@ public class SwerveDrive extends SubsystemBase {
   }
 
   /**
-   * Resets the odometry of the robot to (x=0, y=0, theta=0).
+   * Gets the current gyro using regression formula.
+   * @return Rotation2d object holding current gyro in radians
+   */
+  public Rotation2d getRegGyro() {
+    double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527;
+    return new Rotation2d(regCur * Math.PI / 180);
+  }
+
+  /**
+   * Resets the odometry of the robot to the given pose.
    */
   public void resetOdometry(Pose2d pose) {
-    // m_odometry.resetPosition(pose, m_gyro.getRotation2d());
     m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
   }
 
-  /** Updates the field relative position of the robot. */
-
+  /** Updates the field relative position of the robot.  
+  */
   public void updateOdometry() {
-    m_poseEstimator.update( m_gyro.getRotation2d(), 
+    m_poseEstimator.update( getRegGyro(),
                             modules[0].getState(), 
                             modules[1].getState(), 
                             modules[2].getState(), 
                             modules[3].getState());
-    
-    // m_odometry.update( m_gyro.getRotation2d(), 
-    //                    modules[0].getState(),
-    //                    modules[1].getState(),
-    //                    modules[2].getState(),
-    //                    modules[3].getState());
   
       // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
       // a real robot, this must be calculated based either on latency or timestamps.
@@ -242,6 +231,9 @@ public class SwerveDrive extends SubsystemBase {
     rotTarget = new Rotation2d(0);
   }
 
+  /**
+   * Stop all four swerve modules.
+   */
   public void stopModules() {
     modules[0].stop();
     modules[1].stop();

src/main/java/frc4388/robot/subsystems/SwerveModule.java

@@ -7,6 +7,7 @@ package frc4388.robot.subsystems;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
 import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;
 import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
@@ -15,19 +16,27 @@ import com.ctre.phoenix.sensors.CANCoderConfiguration;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
 
 public class SwerveModule extends SubsystemBase {
-  private WPI_TalonFX driveMotor;
-  private WPI_TalonFX angleMotor;
+  public WPI_TalonFX angleMotor;
+  public WPI_TalonFX driveMotor;
   private CANCoder canCoder;
   public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
 
   private static double kEncoderTicksPerRotation = 4096;
   private SwerveModuleState state;
   private double canCoderFeedbackCoefficient;
+  
+  public long m_currentTime;
+  public long m_lastTime;
+  public double m_deltaTime;
+
+  public double m_currentPos;
+  public double m_lastPos;
 
   /** Creates a new SwerveModule. */
   public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
@@ -48,21 +57,38 @@ public class SwerveModule extends SubsystemBase {
     angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
     angleMotor.configAllSettings(angleTalonFXConfiguration);
 
-    TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
-    driveTalonFXConfiguration.slot0.kP = 0.15;
-    driveTalonFXConfiguration.slot0.kI = 0.0;
-    driveTalonFXConfiguration.slot0.kD = 0.5;
-    driveMotor.configAllSettings(driveTalonFXConfiguration);
+    // TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
+    // driveTalonFXConfiguration.slot0.kP = 0.05;
+    // driveTalonFXConfiguration.slot0.kI = 0.0;
+    // driveTalonFXConfiguration.slot0.kD = 0.0;
+    // driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.IntegratedSensor;
+    driveMotor.configFactoryDefault();
+    driveMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30);
+    driveMotor.configNominalOutputForward(0, 30);
+    driveMotor.configNominalOutputReverse(0, 30);
+    driveMotor.configPeakOutputForward(1, 30);
+    driveMotor.configPeakOutputReverse(-1, 30);
+    driveMotor.configAllowableClosedloopError(0, 0, 30);
+    driveMotor.config_kP(0, 0.5, 30);
+    driveMotor.config_kI(0, 0, 30);
+    driveMotor.config_kD(0, 0, 30);
+    // maybe try a feedforward value?
+    
+    // driveMotor.configAllSettings(driveTalonFXConfiguration);
 
     CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
     canCoderConfiguration.magnetOffsetDegrees = offset;
     canCoder.configAllSettings(canCoderConfiguration);
-  }
 
+    m_currentTime = System.currentTimeMillis();
+    m_lastTime = System.currentTimeMillis();
+
+    m_lastPos = driveMotor.getSelectedSensorPosition();
+  }
 
   private Rotation2d getAngle() {
     // Note: This assumes the CANCoders are setup with the default feedback coefficient
-    // and the sesnor value reports degrees.
+    // and the sensor value reports degrees.
     return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
   }
 
@@ -72,8 +98,7 @@ public class SwerveModule extends SubsystemBase {
    */
   public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
     Rotation2d currentRotation = getAngle();
-    
-    //SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
+    // SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
     state = SwerveModuleState.optimize(desiredState, currentRotation);
 
     // Find the difference between our current rotational position + our new rotational position
@@ -84,29 +109,62 @@ public class SwerveModule extends SubsystemBase {
     // Convert the CANCoder from it's position reading back to ticks
     double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient;
     double desiredTicks = currentTicks + deltaTicks;
+
     if (!ignoreAngle){
       angleMotor.set(TalonFXControlMode.Position, desiredTicks);
     }
 
+    double ftPerSec = Units.metersToFeet(state.speedMetersPerSecond);
+    double normFtPerSec = ftPerSec / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC;
+    // double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69;
+    
+    // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
+    driveMotor.set(normFtPerSec);// - angleMotor.get());
+    // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio between axis = 1/1.75    Ratio of wheel is 5.14/1    ratio of steer is 12.8/1
 
-    double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
-    // driveMotor.set(TalonFXControlMode.Velocity, /*angleMotor.get() + */(Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
-    driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC);
+    // m_currentTime = System.currentTimeMillis();
+    // m_deltaTime = (double) (m_currentTime - m_lastTime);
+    // m_deltaTime = m_deltaTime / 10.0;
+
+    // m_currentPos = driveMotor.getSelectedSensorPosition();
+
+    // double m_desiredCorrectionVel = 3.2 * angleMotor.getSelectedSensorVelocity();
+    // double m_desiredCorrectionPos = (m_deltaTime * m_desiredCorrectionVel) % 2048;
+    // double m_lastPos = (driveMotor.getSelectedSensorPosition() % 2048) - (m_deltaTime * driveMotor.getSelectedSensorVelocity());
+    // double m_actualDesiredPos = m_deltaTime * ((Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
+    
+    // System.out.println("Current Pos: " + driveMotor.getSelectedSensorPosition());
+    // System.out.println("Desired Correction Pos: " + m_desiredCorrectionPos);
+    // System.out.println("Last Pos: " + m_lastPos);
+
+    // driveMotor.set(TalonFXControlMode.Position, 1500/*m_desiredCorrectionPos*/);
+
+    // m_lastTime = m_currentTime;
+    // m_lastPos = m_currentPos;
   }
 
   /**
-   * Returns the current state of the module.
+   * Get current module state.
    *
-   * @return The current state of the module.
+   * @return The current state of the module in m/s.
    */
   public SwerveModuleState getState() {
     // return state;
     return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
   }
 
+  /**
+   * Stop the drive and steer motors of current module.
+   */
   public void stop() {
     driveMotor.set(0);
     angleMotor.set(0);
   }
+  @Override
+  public void periodic(){
+    Rotation2d currentRotation = getAngle();
+    SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
+    SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(), ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
+  }
 
 }

src/main/java/frc4388/robot/subsystems/Turret.java

@@ -0,0 +1,115 @@
+// 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 frc4388.robot.subsystems;
+
+import com.revrobotics.CANSparkMax.ControlType;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMax.SoftLimitDirection;
+
+import java.util.concurrent.TimeoutException;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxLimitSwitch;
+import com.revrobotics.SparkMaxPIDController;
+
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import frc4388.robot.Constants.ShooterConstants;
+import frc4388.robot.commands.Shoot;
+import frc4388.utility.Gains;
+
+public class Turret extends SubsystemBase {
+
+  /** Creates a new Turret. */
+  public BoomBoom m_boomBoomSubsystem;
+  public SwerveDrive m_sDriveSubsystem;
+
+  public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID,
+                                           // MotorType.kBrushless);
+  public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS;
+  SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit;
+  public Gyro m_turretGyro;
+
+  public double m_targetDistance = 0;
+
+  public boolean m_isAimReady = false;
+
+  SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController();
+  public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder();
+
+  // Variables
+  public Turret(CANSparkMax boomBoomRotateMotor) { // Take in rotate motor as an argument
+
+    m_boomBoomRotateMotor = boomBoomRotateMotor;
+    m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController();
+    m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder();
+    m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake);
+
+    m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_boomBoomRightLimit.enableLimitSwitch(true);
+    m_boomBoomLeftLimit.enableLimitSwitch(true);
+    SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled());
+    SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled());
+
+    m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_FORWARD_LIMIT);
+    m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_REVERSE_LIMIT);
+    setTurretSoftLimits(true);
+
+    m_boomBoomRotateMotor.setInverted(false);
+
+    m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP);
+    m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI);
+    m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD);
+    m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF);
+    m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone);
+    m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+
+  /**
+   * Set status of turret motor soft limits.
+   * @param set Boolean to set soft limits to.
+   */
+  public void setTurretSoftLimits(boolean set) {
+    m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, set);
+    m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set);
+  }
+
+  public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) {
+    m_boomBoomSubsystem = subsystem0;
+    m_sDriveSubsystem = subsystem1;
+  }
+
+  public void runTurretWithInput(double input) {
+    m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER);
+  }
+
+  public void runshooterRotatePID(double targetAngle) {
+    targetAngle = targetAngle / ShooterConstants.DEGREES_PER_ROT;
+    m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition);
+  }
+
+  public void resetGyroShooterRotate() {
+    m_boomBoomRotateEncoder.setPosition(0);
+  }
+
+  public double getboomBoomRotatePosition() {
+    return m_boomBoomRotateEncoder.getPosition();
+  }
+
+  public double getBoomBoomAngleDegrees() {
+    return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360
+        / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT;
+  }
+
+}

src/main/java/frc4388/robot/subsystems/Vision.java

@@ -0,0 +1,131 @@
+// 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 frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.networktables.NetworkTableEntry;
+
+import frc4388.robot.Constants.VisionConstants;
+
+public class Vision extends SubsystemBase {
+//setup
+  Turret m_turret;
+  BoomBoom m_boomBoom;
+  Hood m_hood;
+
+NetworkTableEntry xEntry;
+//Aiming
+double turnAmount = 0;
+double xAngle = 0;
+double yAngle = 0;
+double target = 0;
+public double distance;
+public double realDistance;
+public static double fireVel;
+public static double fireAngle;
+
+public double m_hoodTrim;
+public double m_turretTrim;
+public double m_fireAngle;
+
+
+public Vision(Turret aimSubsystem, BoomBoom boomBoom) {
+  m_turret = aimSubsystem;
+  m_boomBoom = boomBoom;
+  m_hood = m_boomBoom.m_hoodSubsystem;
+  //addRequirements(m_turret);
+  limeOff();
+  changePipeline(0);
+  NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
+  NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(3);
+}
+
+public void track(){
+  target = getV();
+  xAngle = getX();
+  yAngle = getY();
+
+  //find distance
+  distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
+  realDistance = (1.09 * distance) - 12.8;
+
+  // if (target == 1.0) { //checks if target is in view
+  //   //aims left and right
+  //   turnAmount = ((xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE);
+  //   if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) {
+  //     turnAmount = 0;
+  //   } 
+  //   else if (turnAmount > 0 && turnAmount < 0.1){
+  //     turnAmount = 0.1;
+  //   }
+  //   else if (turnAmount < 0 && turnAmount > -0.1){
+  //       turnAmount = -0.1;
+  //   }
+  // }
+
+  SmartDashboard.putNumber("Distance to Target", realDistance);
+
+
+  // //start CSV
+
+  // fireVel = m_boomBoom.m_shooterTable.getVelocity(realDistance);
+  // fireAngle = m_boomBoom.m_shooterTable.getHood(realDistance);
+  // //fire angle unknown so far
+  // //end of CSV
+
+  // m_boomBoom.m_fireVel = fireVel;
+  // m_hood.m_fireAngle = fireAngle;
+  // m_turret.m_targetDistance = distance;
+
+  // checkFinished();
+}
+
+public void checkFinished(){
+  if (xAngle < 0.5 && xAngle > -0.5 && target == 1){
+    m_turret.m_isAimReady = true;
+  }
+  else{
+    m_turret.m_isAimReady = false;
+  }
+}
+
+public void limeOff(){
+  NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
+  NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1);
+}
+
+  public void limeOn(){
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
+}
+
+  public void changePipeline(int pipelineId)
+  {
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId);
+  }
+  
+  public double getV()
+  {
+    return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
+  }
+
+  public double getX()
+  {
+    return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
+  }
+
+  public double getY()
+  {
+    return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
+  }
+  @Override
+  public void periodic(){
+    //called once per scheduler run
+  }
+}
+
+