switch to pigeon2 + vector2d docs

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

@@ -24,9 +24,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.RunCommand;
-import frc4388.utility.DesmosServer;
 import frc4388.utility.RobotTime;
 import frc4388.utility.VelocityCorrection;
+import frc4388.utility.desmos.DesmosServer;
 
 /**
  * The VM is configured to automatically run this class, and to call the

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

@@ -297,8 +297,10 @@ public class RobotContainer {
     //     .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
 
     new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
-        .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
-        .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
+        .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)))
+        // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
+        // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender))
+        .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0)));
 
     // new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
     //     .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom));

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

@@ -10,7 +10,7 @@ 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.ctre.phoenix.sensors.WPI_Pigeon2;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMax.IdleMode;
@@ -63,7 +63,7 @@ public class RobotMap {
   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 final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID);
 
   public SwerveModule leftFront;
   public SwerveModule leftBack;

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

@@ -18,7 +18,7 @@ import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Turret;
 import frc4388.robot.subsystems.Vision;
 import frc4388.robot.subsystems.VisionOdometry;
-import frc4388.utility.DesmosServer;
+import frc4388.utility.desmos.DesmosServer;
 
 public class TrackTarget extends CommandBase {
   /** Creates a new TrackTarget. */

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

@@ -12,7 +12,6 @@ import com.revrobotics.CANSparkMax.SoftLimitDirection;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.ExtenderConstants;
-import frc4388.utility.DesmosServer;
 
 public class Extender extends SubsystemBase {
   
@@ -51,7 +50,6 @@ public class Extender extends SubsystemBase {
   public void periodic() {
     // This method will be called once per scheduler run
     SmartDashboard.putNumber("Extender Position", m_extenderMotor.getEncoder().getPosition());
-    DesmosServer.putDouble("ExtenderPosition", m_extenderMotor.getEncoder().getPosition());
   }
 
     /**

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

@@ -4,11 +4,9 @@
 
 package frc4388.robot.subsystems;
 
-import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
-import com.ctre.phoenix.sensors.WPI_PigeonIMU;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
 
 import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -46,14 +44,8 @@ public class SwerveDrive extends SubsystemBase {
       m_backLeftLocation, m_backRightLocation);
 
   public SwerveModule[] modules;
-  public WPI_PigeonIMU m_gyro;
-  protected FusionStatus fstatus = new FusionStatus();
+  public WPI_Pigeon2 m_gyro;
 
-  /*
-   * Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings.
-   * The numbers used
-   * below are robot specific, and should be tuned.
-   */
   public SwerveDrivePoseEstimator m_poseEstimator;
   public SwerveDriveOdometry m_odometry;
 
@@ -65,7 +57,7 @@ public class SwerveDrive extends SubsystemBase {
   private final Field2d m_field = new Field2d();
 
   public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack,
-      WPI_PigeonIMU gyro) {
+      WPI_Pigeon2 gyro) {
 
     m_leftFront = leftFront;
     m_leftBack = leftBack;
@@ -74,14 +66,14 @@ public class SwerveDrive extends SubsystemBase {
     m_gyro = gyro;
 
     modules = new SwerveModule[] {  m_leftFront, m_rightFront, m_leftBack, m_rightBack};
-
+    
     m_poseEstimator = new SwerveDrivePoseEstimator(
         m_gyro.getRotation2d(),
         new Pose2d(),
         m_kinematics,
-        VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),
-        VecBuilder.fill(Units.degreesToRadians(1)),
-        VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)));
+        VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),  // TODO: tune
+        VecBuilder.fill(Units.degreesToRadians(1)),            // TODO: tune
+        VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune
 
     m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
 

src/main/java/frc4388/utility/Vector2D.java

@@ -27,22 +27,50 @@ public class Vector2D extends Vector2d {
     this.angle = Math.atan2(this.y, this.x);
   }
 
+  /**
+   * Add two vectors, component-wise.
+   * @param v1 First vector in the addition.
+   * @param v2 Second vector in the addition.
+   * @return New vector which is the sum.
+   */
   public static Vector2D add(Vector2D v1, Vector2D v2) {
     return new Vector2D(v1.x + v2.x, v1.y + v2.y);
   }
 
+  /**
+   * Subtract two vectors, component-wise.
+   * @param v1 First vector in the subtraction.
+   * @param v2 Second vector in the subtraction.
+   * @return New vector which is the difference.
+   */
   public static Vector2D subtract(Vector2D v1, Vector2D v2) {
     return new Vector2D(v1.x - v2.x, v1.y - v2.y);
   }
 
+  /**
+   * Multiply a vector with a scalar, component-wise.
+   * @param v1 Vector to multiply.
+   * @param v2 Scalar to multiply.
+   * @return New vector which is the product.
+   */
   public static Vector2D multiply(Vector2D v1, double scalar) {
     return new Vector2D(scalar * v1.x, scalar * v1.y);
   }
 
+  /**
+   * Find unit vector.
+   * @return The unit vector.
+   */
   public Vector2D unit() {
     return new Vector2D(this.x / this.magnitude(), this.y / this.magnitude());
   }
 
+  /**
+   * Round a vector to a certain number of places, component-wise.
+   * @param v Vector to round.
+   * @param places Number of places to round to.
+   * @return New rounded vector.
+   */
   public static Vector2D round(Vector2D v, int places) {
     int scale = (int) Math.pow(10, places);
 
@@ -58,7 +86,6 @@ public class Vector2D extends Vector2d {
 
   @Override
   public String toString() {
-    Vector2d test = new Vector2d();
     return ("(" + this.x + ", " + this.y + ")");
   }
 

src/main/java/frc4388/utility/desmos/DesmosServer.java

@@ -1,4 +1,4 @@
-package frc4388.utility;
+package frc4388.utility.desmos;
 
 import java.io.BufferedReader;
 import java.io.IOException;