camera + better auto

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

@@ -132,6 +132,8 @@ public final class Constants {
     // misc
     public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
     public static final Pose2d HUB_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0));
+
+    public static final double AUTO_INCHES_PER_SECOND_AT_FULL_POWER = 134.0;
   }
 
   public static final class SerializerConstants {

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

@@ -262,12 +262,14 @@ public class RobotContainer {
         if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); }
        }, m_robotHood));
 
-       //Climber Manual
+    //    //Climber Manual
     m_robotClimber.setDefaultCommand(
       new RunCommand(() -> {
         if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); }
         if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(-getOperatorController().getRightY()); }
        }, m_robotClimber));
+    // m_robotClimber.setDefaultCommand(
+    //   new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getRightY()), m_robotClimber));
     
     m_robotBoomBoom.setDefaultCommand(
       new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom)
@@ -499,10 +501,9 @@ public class RobotContainer {
     // * 3b. try to only set the drive motors to brake if in auto mode.
     // * 4. try new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive);
     
-    // ! 1.0 input, 1 second: 134 inches
+    // ? 1.0 input, 1 second: 134 inches
-    // ! 0.75 input, 1 second: 48 inches
+    // ? 0.75 input, 1 second: 48 inches
-    // ? positive leftY went left, negative leftY went right?
+    // ! POSITIVE Y IS LEFT, POSITIVE X IS BACKWARDS
-    // TODO: if line 372 is true, switch joystick inputs accordingly
 
     double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate.
     
@@ -519,11 +520,10 @@ public class RobotContainer {
                                                                                  new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
                                                                                  new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0)
                                                                                )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
-    // ! POSITIVE Y IS LEFT, POSITIVE X IS BACKWARDS
   
     // ! DRIVE BACKWARDS AND SHOOT
     return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
-                                       new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, 0.269), // * go backwards three feet
+                                       new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, (3.0 * 12) / SwerveDriveConstants.AUTO_INCHES_PER_SECOND_AT_FULL_POWER),//0.269), // * go backwards three feet
                                        new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake
                                        
                                        weirdAutoShootingGroup, // * shoot
@@ -552,7 +552,7 @@ public class RobotContainer {
     //         new ParallelCommandGroup( new RunCommand(() -> m_robotIntake.runAtOutput(1.0), m_robotIntake), // * run intake all throughout path
 
     //           new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
-    //                                       new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, (40.44 - offset) / distancePerSecond), // * drive to first ball
+    //                                       new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-1.0, 0.0, 0.0, 0.0}, (40.44 - offset) / distancePerSecond), // * drive to first ball
     //                                       // new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, true)), // * brake (see line 376),
     //                                       new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
 

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

@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.cscore.*;
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Camera extends SubsystemBase {
+  /**
+   * Creates a new Camera.
+   * Makes a Camera and sends the stream to a CameraServer, to be viewed in Shuffle Board.
+   * @param name Name of the Camera in Shuffle Board.
+   * @param id USB Id of the Camera.
+   * @param width Resolution width.
+   * @param height Resolution height.
+   * @param brightness Percent brightness of the stream.
+   */
+  public Camera(String name, int id, int width, int height, int brightness) {
+    try{
+      UsbCamera cam = new UsbCamera(name, id);
+      cam.setResolution(width, height);
+      cam.setBrightness(brightness);
+      cam.setFPS(10);
+      VideoSource camera = cam;
+      CameraServer.startAutomaticCapture(camera);
+    } 
+    catch(Exception e) {
+      System.err.println("Camera broken, pls nerf");
+      e.printStackTrace();
+    }
+  }
+
+  @Override
+  public void periodic() {}
+}