small changes shtuf

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

@@ -162,15 +162,6 @@ public class Robot extends TimedRobot {
     // robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
-    
-    // VelocityCorrection vc = new VelocityCorrection(m_robotContainer.m_robotSwerveDrive, m_robotContainer.m_robotBoomBoom);
-    // System.out.println("Position: " + vc.position);
-    // System.out.println("Velocity: " + vc.cartesianVelocity);
-    // System.out.println("Target: " + vc.target.toString());
-
-
-    //SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition());
-    //SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition());
 
     // odo chooser stuff
     addOdoChoices(new Pose2d(1, 1, new Rotation2d(Math.PI/6)), 
@@ -178,12 +169,6 @@ public class Robot extends TimedRobot {
                   new Pose2d(1, 3, new Rotation2d(Math.PI/4)));
     updateOdoChooser();
     SmartDashboard.putData("Odometry Chooser", odoChooser);
-
-    // 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());
   }
 
   public void updateOdoChooser() {    

src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java

@@ -53,7 +53,7 @@ public class ManageStorage extends CommandBase {
 
       // * CommandScheduler.getInstance().schedule(new ExampleCommand());
       // * new ExampleCommand().schedule();
-      // * new ExampleCommand().execute();
+      // * new ExampleCommand().execute(); (accompanied by initialize and onFinished)
       
       new SpitOutWrongColor(this.storage, this.drum, this.turret); // ? is this how you run a command inside a command
     }

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

@@ -169,9 +169,9 @@ public class SwerveDrive extends SubsystemBase {
 
     // chassis speeds
     // TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds
-    SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
-    SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
-    SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
+    // SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
+    // SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
+    // SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
   }
 
   /**

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

@@ -45,10 +45,8 @@ public class Turret extends SubsystemBase {
 
     m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
     m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
-    m_boomBoomRightLimit.enableLimitSwitch(true);
-    m_boomBoomLeftLimit.enableLimitSwitch(true);
+    setTurretLimitSwitches(true);
 
-    // setTurretLimitSwitches(false);
     // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled());
     // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled());
 
@@ -74,8 +72,13 @@ public class Turret extends SubsystemBase {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+
+    
     SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition());
     SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT);
+    SmartDashboard.putBoolean("Left Limit Switch Pressed", m_boomBoomLeftLimit.isPressed());
+    SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed());
+
     if (m_boomBoomLeftLimit.isPressed()) m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_LIMIT - 2);
     if (m_boomBoomRightLimit.isPressed()) m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_LIMIT + 2);
   }

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

@@ -57,6 +57,16 @@ public class Vector2D extends Vector2d {
     return new Vector2D(scalar * v1.x, scalar * v1.y);
   }
 
+    /**
+   * Divide a vector with a scalar, component-wise.
+   * @param v1 Vector to divide.
+   * @param v2 Scalar to divide.
+   * @return New vector which is the division.
+   */
+  public static Vector2D divide(Vector2D v1, double scalar) {
+    return new Vector2D(v1.x / scalar, v1.y / scalar);
+  }
+
   /**
    * Find unit vector.
    * @return The unit vector.