Merge branch 'origin/swerve' into robot-logger

2026-06-09 00:38:05 -06:00 · 2022-03-01 17:56:55 -07:00
parent fa7368305b b01d79ae70
commit 1e573f18e4
25 changed files with 1444 additions and 98 deletions
@@ -1,4 +1,4 @@
-import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO 
+import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO
 plugins {
    id "java"
@@ -28,7 +28,7 @@ deploy {
                frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
                // Enable visualvm
-                jvmArgs << "-Dcom.sun.management.jmxremote=true -Dcom.sun.management.jmxremote.port=1099 -Dcom.sun.management.jmxremote.local.only=false -Dcom.sun.management.jmxremote.ssl=false -Dcom.sun.management.jmxremote.authenticate=false -Djava.rmi.server.hostname=10.43.88.2"                     
+                //jvmArgs << "-Dcom.sun.management.jmxremote=true -Dcom.sun.management.jmxremote.port=1099 -Dcom.sun.management.jmxremote.local.only=false -Dcom.sun.management.jmxremote.ssl=false -Dcom.sun.management.jmxremote.authenticate=false -Djava.rmi.server.hostname=10.43.88.2"                     
                }
                // Static files artifact
@@ -0,0 +1,3 @@
 Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds)
 0             ,16            ,12000
 999           ,28.8          ,12000
@@ -0,0 +1 @@
 {"waypoints":[{"anchorPoint":{"x":6.888836249581911,"y":5.215172677538834},"prevControl":null,"nextControl":{"x":6.537459367052471,"y":6.518965846916928},"holonomicAngle":106.3895403340348,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.8254588419278255,"y":7.8320057763675734},"prevControl":{"x":5.95491348285958,"y":6.759381608647329},"nextControl":{"x":5.95491348285958,"y":6.759381608647329},"holonomicAngle":-136.27303002005684,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.085718036603518,"y":6.18608248452099},"prevControl":{"x":5.222728278411091,"y":6.456690985532331},"nextControl":{"x":4.768933589337623,"y":5.5604025113498725},"holonomicAngle":-109.50244850666218,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.77803093326875,"y":3.4953054389840252},"prevControl":{"x":5.623399779842569,"y":3.7042882657946703},"nextControl":{"x":2.9363292766883204,"y":3.040020127851594},"holonomicAngle":-30.256437163529373,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.7486154042034336,"y":2.667447141251462},"prevControl":{"x":1.8144326730676037,"y":2.686839729398941},"nextControl":{"x":0.7297128839172015,"y":2.3672347915242677},"holonomicAngle":-75.06858282186249,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.2129730034385093,"y":1.1845800804778424},"prevControl":{"x":1.2001001569914929,"y":1.2299758967179297},"nextControl":{"x":1.530279149856384,"y":0.06560677694911808},"holonomicAngle":-39.472459848343846,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.160119378376086,"y":1.9305622828301685},"prevControl":{"x":5.131309514738882,"y":1.9153315905888442},"nextControl":{"x":6.588402375563037,"y":2.6856418291137163},"holonomicAngle":36.02737338510347,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.60730489584927,"y":0.37491647060743843},"prevControl":{"x":7.329902093852659,"y":0.5875919521381767},"nextControl":{"x":7.884707697845882,"y":0.16224098907670015},"holonomicAngle":-41.00908690157027,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":8.617110072204374,"y":0.9389517943373179},"prevControl":{"x":8.535233976824232,"y":0.12019084053588047},"nextControl":{"x":8.698986167584517,"y":1.7577127481387553},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":9.144756020209744,"y":0.31123506308954896},"prevControl":{"x":9.199340083796505,"y":0.7206155399902687},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]}
@@ -0,0 +1 @@
 {"waypoints":[{"anchorPoint":{"x":1.0,"y":3.0},"prevControl":null,"nextControl":{"x":2.3149413387434365,"y":2.75478019310469},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.143744996350752,"y":3.8428026223141054},"prevControl":{"x":1.0069994823533945,"y":5.5442845062905315},"nextControl":{"x":7.478686694138279,"y":2.0338121072780666},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.870800551727686,"y":4.710905624342894},"prevControl":{"x":6.870800551727686,"y":4.710905624342894},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]}
@@ -0,0 +1 @@
 {"waypoints":[{"anchorPoint":{"x":6.620680207650139,"y":5.35387407853639},"prevControl":null,"nextControl":{"x":6.047381083523801,"y":5.557302800000574},"holonomicAngle":169.99202019855858,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.020990716136325,"y":6.167588964393128},"prevControl":{"x":5.409354638931585,"y":5.5018222396012515},"nextControl":{"x":5.409354638931585,"y":5.5018222396012515},"holonomicAngle":129.4995885297982,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.141198597001524,"y":1.9325728539114686},"prevControl":{"x":5.085718036602201,"y":3.2825998236283294},"nextControl":null,"holonomicAngle":-77.66091272167375,"isReversal":false,"velOverride":null,"isLocked":false}]}
@@ -0,0 +1 @@
 {"waypoints":[{"anchorPoint":{"x":8.109408578365308,"y":2.117508055242545},"prevControl":null,"nextControl":{"x":8.192629418964295,"y":1.470234850583776},"holonomicAngle":-92.202598161766,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.637823814971061,"y":0.36987040266386867},"prevControl":{"x":8.775175303157184,"y":0.48083152346251423},"nextControl":{"x":6.123786635522426,"y":0.22215945832741724},"holonomicAngle":178.6360724683971,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.141198597001524,"y":1.9048325737118068},"prevControl":{"x":5.61278336039577,"y":1.729144132447284},"nextControl":null,"holonomicAngle":143.8418145601917,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":null,"maxAcceleration":5.0,"isReversed":null}
@@ -0,0 +1 @@
 {"waypoints":[{"anchorPoint":{"x":6.777875128781554,"y":2.4688849377715907},"prevControl":null,"nextControl":{"x":5.797718561726847,"y":2.043533974710114},"holonomicAngle":-156.37062226934333,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.2298190888492497,"y":1.174338528454054},"prevControl":{"x":1.904832573707682,"y":1.0171436073226392},"nextControl":null,"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false}]}
@@ -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
+    // offsets are in degrees
-    public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
+    // NATHAN if you truncate or round or simplify these i will cry
-    public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
+    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 LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.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 RIGHT_BACK_ENCODER_OFFSET = 0.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_VELOCITY = 5.0;
-    public static final double PATH_MAX_ACCELERATION = 50;
+    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;
  }
 }
@@ -35,5 +35,4 @@ public final class Main {
  }
 }
-
+// hi ryan -aarav
 // hi ryan
@@ -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
@@ -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(
+  public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
      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)
+    // "XboxController.Button.kBack" was undefined yet, 7 works just fine
-        // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.Y_BUTTON)
+    new JoystickButton(getDriverController(), 7)
-        .whenPressed(m_robotSwerveDrive.m_gyro::reset);
+      .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);
  }
@@ -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);
+    NeutralMode mode = NeutralMode.Coast;
-    leftFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
+    leftFrontSteerMotor.setNeutralMode(mode);
-    rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
+    leftFrontWheelMotor.setNeutralMode(mode);//Coast
-    rightFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
+    rightFrontSteerMotor.setNeutralMode(mode);
-    leftBackSteerMotor.setNeutralMode(NeutralMode.Brake);
+    rightFrontWheelMotor.setNeutralMode(mode);//Coast
-    leftBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
+    leftBackSteerMotor.setNeutralMode(mode);
-    rightBackSteerMotor.setNeutralMode(NeutralMode.Brake);
+    leftBackWheelMotor.setNeutralMode(mode);//Coast
-    rightBackWheelMotor.setNeutralMode(NeutralMode.Brake);//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);
+    leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(),
-    //leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
-    //rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.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
  }
 }
@@ -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;
  }
 }
@@ -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;
  }
 }
@@ -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));
  }
 }
@@ -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;
  }
 }
@@ -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);
+    updateOdometry();
-    // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    // SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
-    // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+
-    m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
+    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();
@@ -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;
+  public WPI_TalonFX angleMotor;
-  private 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();
+    // TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
-    driveTalonFXConfiguration.slot0.kP = 0.15;
+    // driveTalonFXConfiguration.slot0.kP = 0.05;
-    driveTalonFXConfiguration.slot0.kI = 0.0;
+    // driveTalonFXConfiguration.slot0.kI = 0.0;
-    driveTalonFXConfiguration.slot0.kD = 0.5;
+    // driveTalonFXConfiguration.slot0.kD = 0.0;
-    driveMotor.configAllSettings(driveTalonFXConfiguration);
+    // 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);
+    // m_currentTime = System.currentTimeMillis();
-    // driveMotor.set(TalonFXControlMode.Velocity, /*angleMotor.get() + */(Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
+    // m_deltaTime = (double) (m_currentTime - m_lastTime);
-    driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC);
+    // 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);
  }
 }
@@ -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;
  }
 }
@@ -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
  }
 }
@@ -0,0 +1,233 @@
 package frc4388.utility;
 import java.awt.Color;
 import java.io.BufferedReader;
 import java.io.IOException;
 import java.lang.invoke.MethodHandleProxies;
 import java.lang.invoke.MethodHandles;
 import java.lang.invoke.MethodType;
 import java.lang.reflect.Array;
 import java.lang.reflect.Field;
 import java.lang.reflect.Modifier;
 import java.nio.file.Files;
 import java.nio.file.Path;
 import java.text.MessageFormat;
 import java.util.ArrayList;
 import java.util.HashMap;
 import java.util.List;
 import java.util.Map;
 import java.util.Objects;
 import java.util.function.BiConsumer;
 import java.util.function.Function;
 import java.util.function.IntFunction;
 import java.util.function.Predicate;
 import java.util.function.Supplier;
 import java.util.regex.Pattern;
 import java.util.stream.Collectors;
 import java.util.stream.IntStream;
 import java.util.stream.Stream;
 public class CSV<R> {
  private static final Pattern SANITIZER = Pattern.compile("[^$\\w,]");
  private final Supplier<R> generator;
  private final IntFunction<R[]> arrayGenerator;
  private final Map<String, BiConsumer<R, String>> setters;
  /**
   * A binary string operator to be applied to the entire header of the CSV.
   */
  protected String headerSanitizer(final String header) {
    return SANITIZER.matcher(header).replaceAll("");
  }
  /**
   * A binary string operator to be applied to each name in the header of the CSV.
   */
  protected String nameProcessor(final String name) {
    return Character.toLowerCase(name.charAt(0)) + name.substring(1);
  }
  /**
   * Creates a new {@code CSV} instance and prepares for populating the fields of objects created by
   * the given generator. Private fields and fields of primitive types are not supported.
   * @param generator a parameterless supplier which produces a new object with any number of fields
   *                  corresponding to header names from a CSV file. The first character of the names
   *                  from the header in the CSV file will be made lowercase and invalid characters
   *                  will be removed to match Java naming conventions.
   * @see #read(Path)
   */
  @SuppressWarnings("unchecked")
  public CSV(final Supplier<R> generator) {
    final Class<?> clazz = generator.get().getClass();
    final Map<Class<?>, Function<String, ?>> fieldParsers = new HashMap<>();
    this.arrayGenerator = size -> (R[]) Array.newInstance(clazz, size);
    this.generator = generator;
    this.setters = new HashMap<>();
    for (final Field field : clazz.getFields()) {
      final Function<String, ?> parser = Modifier.isStatic(field.getModifiers()) ? null : fieldParsers.computeIfAbsent(field.getType(), CSV::getTypeParser);
      if (parser != null)
        this.setters.put(field.getName(), (final R obj, final String rawValue) -> {
          try {
            field.set(obj, rawValue.isEmpty() ? null : parser.apply(rawValue));
          } catch (final IllegalAccessException e) {
            throw new RuntimeException(e);
          }
        });
    }
  }
  /**
   * Reads and parses the contents of the given CSV file, and returns an array filled with populated
   * objects created with the previously given generator. Cells are parsed using their corresponding
   * field's {@code valueOf(String)} function.
   * @param path the path to a CSV file
   * @return the parsed data from the CSV file
   * @throws IOException if an I/O error occurs opening the file
   */
  @SuppressWarnings("unchecked")
  public R[] read(final Path path) throws IOException {
    try (final BufferedReader reader = Files.newBufferedReader(path)) {
      final BiConsumer<R, String>[] fieldSetters = Stream.of(headerSanitizer(reader.readLine()).split(",")).map(this::nameProcessor).map(setters::get).toArray(BiConsumer[]::new);
      final Stream<String> lines = reader.lines();
      return lines.filter(Predicate.not(String::isBlank)).map(line -> deserializeRecordString(line, fieldSetters, generator.get())).toArray(this.arrayGenerator);
    }
  }
  @SuppressWarnings("unchecked")
  private static Function<String, ?> getTypeParser(final Class<?> type) {
    try {
      return type.isAssignableFrom(String.class) ? Function.identity() : MethodHandleProxies.asInterfaceInstance(Function.class, MethodHandles.publicLookup().findStatic(type, "valueOf", MethodType.methodType(type, String.class)));
    } catch (final NoSuchMethodException | IllegalAccessException e) {
      return null;
    }
  }
  private static <R> R deserializeRecordString(final String recordString, final BiConsumer<R, String>[] fieldParseSetters, final R object) {
    final int recordStringLength = recordString.length();
    int fieldBeginIndex = 0, tryFieldEndFromIndex = 0, i = 0;
    while (tryFieldEndFromIndex < recordStringLength && i < fieldParseSetters.length) {
      final int tryFieldEndIndex = recordString.indexOf(',', tryFieldEndFromIndex);
      String field = recordString.substring(fieldBeginIndex, tryFieldEndIndex == -1 ? recordStringLength : tryFieldEndIndex).strip();
      if (!field.isEmpty() && (tryFieldEndFromIndex != fieldBeginIndex || field.charAt(0) == '"')) {
        final int fieldLength = field.length();
        if (countTrailing(field, '"') % 2 == 0) {
          tryFieldEndFromIndex = tryFieldEndIndex + 1;
          continue;
        } else
          field = field.substring(1, fieldLength - 1).replace("\"\"", "\"");
      }
      final BiConsumer<R, String> setter = fieldParseSetters[i];
      if (setter != null)
        setter.accept(object, field);
      tryFieldEndFromIndex = fieldBeginIndex = tryFieldEndIndex + 1;
      i++;
    }
    return object;
  }
  private static int countTrailing(final String str, final char c) {
    final int l = str.length();
    int count = 0;
    while (str.charAt(l - count - 1) == c && count < l)
      count++;
    return count;
  }
  public static final class ReflectionTable {
    public static <T> String create(final T[] objects, boolean colored) {
      final Field[] fields = Stream.of(objects).flatMap(object -> Stream.of(object.getClass().getFields())).distinct().toArray(Field[]::new);
      final List<List<ReflectionTable>> rows = new ArrayList<>();
      rows.add(Stream.of(fields).map(ReflectionTable::new).collect(Collectors.toList()));
      rows.addAll(Stream.of(objects).map(obj -> Stream.of(fields).map(field -> new ReflectionTable(obj, field)).collect(Collectors.toList())).collect(Collectors.toList()));
      final int[] columnWidths = rows.stream().map(row -> row.stream().map(cell -> cell.string).mapToInt(String::length).toArray()).reduce(new int[fields.length], (result, row) -> IntStream.range(0, row.length).map(i -> Math.max(result[i], row[i])).toArray());
      if (colored)
        IntStream.range(0, fields.length).forEach(i -> {
          final var columnSummaryStatistics = rows.stream().skip(1).mapToDouble(row -> row.get(i).getValue().doubleValue()).summaryStatistics();
          rows.stream().skip(1).forEach(row -> row.get(i).colorByValue(columnSummaryStatistics.getMin(), columnSummaryStatistics.getMax()));
        });
      MessageFormat formatFormat = new MessageFormat(colored ? "{2} %{0}{1}s {3}" : " %{0}{1}s ");
      return rows.stream().map(row -> IntStream.range(0, row.size()).mapToObj(i -> String.format(formatFormat.format(new Object[] { row.get(i).padRight ? "-" : "", columnWidths[i], row.get(i).escape, RESET_STYLE }), row.get(i).string)).collect(Collectors.joining("|"))).collect(Collectors.joining(LF));
    }
    private static final Color GRADIENT_MIN = new Color(0x00, 0x99, 0x00);
    private static final Color GRADIENT_MAX = new Color(0x66, 0xFF, 0x66);
    private static final String CONTROL = "\033";
    private static final String CSI = "[";
    private static final String LF = "\n";
    private static final String RESET = "0";
    private static final String BOLD = "1";
    private static final String ITALIC = "3";
    private static final String UNDERLINE = "4";
    private static final String BACKGROUND_RED = "41";
    private static final String FOREGROUND = "38";
    private static final String BACKGROUND = "48";
    private static final String TRUECOLOR = "2";
    private static final String SEPARATOR = ";";
    private static final String SGR = "m";
    private static final String HEADER_STYLE = CONTROL + CSI + BOLD + SEPARATOR + UNDERLINE + SGR;
    private static final String NULL_STYLE = CONTROL + CSI + ITALIC + SGR;
    private static final String ERROR_STYLE = CONTROL + CSI + ITALIC + SGR + CONTROL + CSI + BACKGROUND_RED + SGR;
    private static final String RESET_STYLE = CONTROL + CSI + RESET + SGR;
    private Object value;
    private String string;
    private boolean padRight;
    private String escape;
    private ReflectionTable(final Object obj, final Field field) {
      try {
        value = field.get(obj);
        string = Objects.toString(value);
        padRight = !Number.class.isAssignableFrom(field.getType());
        escape = Objects.isNull(value) ? NULL_STYLE : "";
      } catch (final IllegalAccessException | IllegalArgumentException e) {
        value = null;
        string = e.getClass().getSimpleName();
        padRight = false;
        escape = ERROR_STYLE;
      }
    }
    private ReflectionTable(final Field field) {
      value = null;
      string = field.getName();
      padRight = true;
      escape = HEADER_STYLE;
    }
    private Number getValue() {
      return padRight ? Objects.hashCode(string) : Objects.requireNonNullElse((Number) value, 0);
    }
    private void colorByValue(final Number min, final Number max) {
      if (Objects.nonNull(value)) {
        final double range = max.doubleValue() - min.doubleValue();
        final double normal = range == 0 ? 0 : (getValue().doubleValue() - min.doubleValue()) / range;
        final Color color = new Color(range(normal, GRADIENT_MIN.getRed(), GRADIENT_MAX.getRed()), range(normal, GRADIENT_MIN.getGreen(), GRADIENT_MAX.getGreen()), range(normal, GRADIENT_MIN.getBlue(), GRADIENT_MAX.getBlue()));
        escape += (contrastRatio(color, Color.BLACK) > contrastRatio(Color.WHITE, color) ? colorTo24BitSGR(Color.BLACK, false) : colorTo24BitSGR(Color.WHITE, false)) + colorTo24BitSGR(color, true);
      }
    }
    private static String colorTo24BitSGR(final Color color, final boolean background) {
      return CONTROL + CSI + (background ? BACKGROUND : FOREGROUND) + SEPARATOR + TRUECOLOR + SEPARATOR + color.getRed() + SEPARATOR + color.getGreen() + SEPARATOR + color.getBlue() + SGR;
    }
    private static int range(final double normal, final int min, final int max) {
      return (int) (normal * (max - min) + min);
    }
    /* https://www.w3.org/TR/WCAG20/#contrast-ratiodef */
    private static float contrastRatio(final Color lighter, final Color darker) {
      return (relativeLuminance(lighter) + 0.05f) / (relativeLuminance(darker) + 0.05f);
    }
    /* https://www.w3.org/TR/2008/REC-WCAG20-20081211/#relativeluminancedef */
    private static float relativeLuminance(final Color color) {
      final float[] components = color.getRGBComponents(null);
      final float r = components[0] <= 0.03928f ? components[0] / 12.92f : (float) Math.pow((components[0] + 0.055f) / 1.055f, 2.4f);
      final float g = components[1] <= 0.03928f ? components[1] / 12.92f : (float) Math.pow((components[1] + 0.055f) / 1.055f, 2.4f);
      final float b = components[2] <= 0.03928f ? components[2] / 12.92f : (float) Math.pow((components[2] + 0.055f) / 1.055f, 2.4f);
      return 0.2126f * r + 0.7152f * g + 0.0722f * b;
    }
  }
 }
@@ -0,0 +1,113 @@
 package frc4388.commands;
 import org.junit.Test;
 import frc4388.robot.commands.AimToCenter;
 import org.junit.Assert;
 public class AimToCenterTest {
    private static final double DELTA = 1e-15;
    @Test
    public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() {
        boolean output;
            //20 deg
        output = AimToCenter.isHardwareDeadzone(20.);
        Assert.assertFalse(output);
            //-10 deg
        output = AimToCenter.isHardwareDeadzone(-10.);
        Assert.assertTrue(output);
            //-1 deg
        output = AimToCenter.isHardwareDeadzone(-1.);
        Assert.assertTrue(output);
            //341 deg
        output = AimToCenter.isHardwareDeadzone(341.);
        Assert.assertTrue(output);
            //340 deg
        output = AimToCenter.isHardwareDeadzone(340.);
        Assert.assertFalse(output);
            //0 deg
        output = AimToCenter.isHardwareDeadzone(0.);
        Assert.assertFalse(output);
            //200 deg
        output = AimToCenter.isHardwareDeadzone(200.);
        Assert.assertFalse(output);
            //2000000 deg
        output = AimToCenter.isHardwareDeadzone(2000000.);
        Assert.assertTrue(output);
            //NaN deg
        output = AimToCenter.isHardwareDeadzone(Double.NaN);
        Assert.assertFalse(output);
    }
    @Test 
    public void givenOdometry_whenCalculateAngleToCenter_thenReturnAngleToCenter() {
        double actual;
        double expected;
            //(5,5) Gyro = 0 deg
        actual = AimToCenter.angleToCenter(5., 5., 0.);
        expected = 180. + 45.;
        Assert.assertEquals(expected, actual, DELTA);
            //(-5,5) Gyro = 0 deg
        actual = AimToCenter.angleToCenter(-5.0, 5., 0.);
        expected = 180. + 90. + 45.;
        Assert.assertEquals(expected, actual, DELTA);
            //(-5,-5) Gyro = 0 deg
        actual = AimToCenter.angleToCenter(-5.0, -5., 0.);
        expected = 45.;
        Assert.assertEquals(expected, actual, DELTA);
            //(5,-5) Gyro = 0 deg
        actual = AimToCenter.angleToCenter(5., -5., 0.);
        expected = 90. + 45.;
        Assert.assertEquals(expected, actual, DELTA);
            //(0,0) Gyro = 0 deg
        actual = AimToCenter.angleToCenter(0., 0., 0.);
        Assert.assertNotNull(actual);
            //(5,5) Gyro = 180 deg
        actual = AimToCenter.angleToCenter(5., 5., 180.);
        expected = 45.;
        Assert.assertEquals(expected, actual, DELTA);
            //(100,100) Gyro = 90 deg
        actual = AimToCenter.angleToCenter(100., 100., 90.);
        expected = 90. + 45.;
        Assert.assertEquals(expected, actual, DELTA);
            //(null,5) Gyro = 0 deg
        actual = AimToCenter.angleToCenter(Double.NaN, 5., 0.);
        expected = Double.NaN;
        Assert.assertEquals(expected, actual, DELTA);
            //(null,null) Gyro = 0 deg
        actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, 0.);
        expected = Double.NaN;
        Assert.assertEquals(expected, actual, DELTA);
            //(null,null) Gyro = null deg
        actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, Double.NaN);
        expected = Double.NaN;
        Assert.assertEquals(expected, actual, DELTA);
            //(5,5) Gyro = -20 deg
        actual = AimToCenter.angleToCenter(5., -5., -45.);
        expected = 180.;
        Assert.assertEquals(expected, actual, DELTA);
    }
 }
@@ -6,6 +6,7 @@ package frc4388.robot.subsystems;
 import static org.junit.Assert.assertEquals;
 // import static org.mockito.Mockito.mock;
 import static org.mockito.Mockito.mock;
 import org.junit.Test;
@@ -17,6 +18,9 @@ import frc4388.utility.LEDPatterns;
 * Add your docs here.
 */
 public class LEDSubsystemTest {
  // Arrange
  Spark ledController = mock(Spark.class);
  LED led = new LED(ledController);
  @Test
  public void testConstructor() {
    // Arrange
@@ -0,0 +1,73 @@
 {
    "fileName": "REVLib.json",
    "name": "REVLib",
    "version": "2022.1.1",
    "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
    "mavenUrls": [
        "https://maven.revrobotics.com/"
    ],
    "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json",
    "javaDependencies": [
        {
            "groupId": "com.revrobotics.frc",
            "artifactId": "REVLib-java",
            "version": "2022.1.1"
        }
    ],
    "jniDependencies": [
        {
            "groupId": "com.revrobotics.frc",
            "artifactId": "REVLib-driver",
            "version": "2022.1.1",
            "skipInvalidPlatforms": true,
            "isJar": false,
            "validPlatforms": [
                "windowsx86-64",
                "windowsx86",
                "linuxaarch64bionic",
                "linuxx86-64",
                "linuxathena",
                "linuxraspbian",
                "osxx86-64"
            ]
        }
    ],
    "cppDependencies": [
        {
            "groupId": "com.revrobotics.frc",
            "artifactId": "REVLib-cpp",
            "version": "2022.1.1",
            "libName": "REVLib",
            "headerClassifier": "headers",
            "sharedLibrary": false,
            "skipInvalidPlatforms": true,
            "binaryPlatforms": [
                "windowsx86-64",
                "windowsx86",
                "linuxaarch64bionic",
                "linuxx86-64",
                "linuxathena",
                "linuxraspbian",
                "osxx86-64"
            ]
        },
        {
            "groupId": "com.revrobotics.frc",
            "artifactId": "REVLib-driver",
            "version": "2022.1.1",
            "libName": "REVLibDriver",
            "headerClassifier": "headers",
            "sharedLibrary": false,
            "skipInvalidPlatforms": true,
            "binaryPlatforms": [
                "windowsx86-64",
                "windowsx86",
                "linuxaarch64bionic",
                "linuxx86-64",
                "linuxathena",
                "linuxraspbian",
                "osxx86-64"
            ]
        }
    ]
 }
		`@@ -0,0 +1 @@`
							{"waypoints":[{"anchorPoint":{"x":6.888836249581911,"y":5.215172677538834},"prevControl":null,"nextControl":{"x":6.537459367052471,"y":6.518965846916928},"holonomicAngle":106.3895403340348,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.8254588419278255,"y":7.8320057763675734},"prevControl":{"x":5.95491348285958,"y":6.759381608647329},"nextControl":{"x":5.95491348285958,"y":6.759381608647329},"holonomicAngle":-136.27303002005684,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.085718036603518,"y":6.18608248452099},"prevControl":{"x":5.222728278411091,"y":6.456690985532331},"nextControl":{"x":4.768933589337623,"y":5.5604025113498725},"holonomicAngle":-109.50244850666218,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.77803093326875,"y":3.4953054389840252},"prevControl":{"x":5.623399779842569,"y":3.7042882657946703},"nextControl":{"x":2.9363292766883204,"y":3.040020127851594},"holonomicAngle":-30.256437163529373,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.7486154042034336,"y":2.667447141251462},"prevControl":{"x":1.8144326730676037,"y":2.686839729398941},"nextControl":{"x":0.7297128839172015,"y":2.3672347915242677},"holonomicAngle":-75.06858282186249,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.2129730034385093,"y":1.1845800804778424},"prevControl":{"x":1.2001001569914929,"y":1.2299758967179297},"nextControl":{"x":1.530279149856384,"y":0.06560677694911808},"holonomicAngle":-39.472459848343846,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.160119378376086,"y":1.9305622828301685},"prevControl":{"x":5.131309514738882,"y":1.9153315905888442},"nextControl":{"x":6.588402375563037,"y":2.6856418291137163},"holonomicAngle":36.02737338510347,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.60730489584927,"y":0.37491647060743843},"prevControl":{"x":7.329902093852659,"y":0.5875919521381767},"nextControl":{"x":7.884707697845882,"y":0.16224098907670015},"holonomicAngle":-41.00908690157027,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":8.617110072204374,"y":0.9389517943373179},"prevControl":{"x":8.535233976824232,"y":0.12019084053588047},"nextControl":{"x":8.698986167584517,"y":1.7577127481387553},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":9.144756020209744,"y":0.31123506308954896},"prevControl":{"x":9.199340083796505,"y":0.7206155399902687},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]}
		`@@ -0,0 +1 @@`
							{"waypoints":[{"anchorPoint":{"x":1.0,"y":3.0},"prevControl":null,"nextControl":{"x":2.3149413387434365,"y":2.75478019310469},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.143744996350752,"y":3.8428026223141054},"prevControl":{"x":1.0069994823533945,"y":5.5442845062905315},"nextControl":{"x":7.478686694138279,"y":2.0338121072780666},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.870800551727686,"y":4.710905624342894},"prevControl":{"x":6.870800551727686,"y":4.710905624342894},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]}
		`@@ -0,0 +1 @@`
							{"waypoints":[{"anchorPoint":{"x":6.620680207650139,"y":5.35387407853639},"prevControl":null,"nextControl":{"x":6.047381083523801,"y":5.557302800000574},"holonomicAngle":169.99202019855858,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.020990716136325,"y":6.167588964393128},"prevControl":{"x":5.409354638931585,"y":5.5018222396012515},"nextControl":{"x":5.409354638931585,"y":5.5018222396012515},"holonomicAngle":129.4995885297982,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.141198597001524,"y":1.9325728539114686},"prevControl":{"x":5.085718036602201,"y":3.2825998236283294},"nextControl":null,"holonomicAngle":-77.66091272167375,"isReversal":false,"velOverride":null,"isLocked":false}]}
		`@@ -0,0 +1 @@`
							{"waypoints":[{"anchorPoint":{"x":8.109408578365308,"y":2.117508055242545},"prevControl":null,"nextControl":{"x":8.192629418964295,"y":1.470234850583776},"holonomicAngle":-92.202598161766,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.637823814971061,"y":0.36987040266386867},"prevControl":{"x":8.775175303157184,"y":0.48083152346251423},"nextControl":{"x":6.123786635522426,"y":0.22215945832741724},"holonomicAngle":178.6360724683971,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.141198597001524,"y":1.9048325737118068},"prevControl":{"x":5.61278336039577,"y":1.729144132447284},"nextControl":null,"holonomicAngle":143.8418145601917,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":null,"maxAcceleration":5.0,"isReversed":null}
		`@@ -0,0 +1 @@`
							`{"waypoints":[{"anchorPoint":{"x":6.777875128781554,"y":2.4688849377715907},"prevControl":null,"nextControl":{"x":5.797718561726847,"y":2.043533974710114},"holonomicAngle":-156.37062226934333,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.2298190888492497,"y":1.174338528454054},"prevControl":{"x":1.904832573707682,"y":1.0171436073226392},"nextControl":null,"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false}]}`