Merge pull request #34 from grt192/writeup

Writeup

src/main/java/frc/robot/RobotContainer.java

@@ -141,15 +141,26 @@ public RobotContainer() {
      * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
      * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
      * 
-     * Controller 1 (driver):
-     * Joysticks -> car drive -- left Y -> foward, right X -> angular (tank)
-     * Triggers -> manual override intake -- left backwards, right forwards (intake)
-     * A button -> lower intake (intake)
-     * B button -> raise intake (intake)
+     * Controller 1 (drive):
+     * Joysticks (held)       -> car drive -- left Y axis controls foward power, right X axis controls angular power
+     * X button (pressed)     -> toggle turret retracted mode
+     * Left trigger (held)    -> freeze turret
+     * Right bumper (pressed) -> manual reset turret r to 140 in. (edge of tarmac)
+     * Right bumper (held)    -> slow mode; scales down drive powers for more precise inputs
      * 
-     * Controller 2 (mechanisms):
-     * A button -> request shot (internals)
-     * X button -> start climb sequence (climb)
+     * Controller 2 (mech):
+     * A button (pressed)           -> request shot
+     * B button (pressed)           -> toggle intake position (raise / lower)
+     * X button (pressed)           -> reset turret offsets
+     * Y button (pressed)           -> toggle turret low hub mode
+     * Triggers (held)              -> run intake -- left backwards, right forwards
+     * Left joystick Y axis (held)  -> offset intake position if it fails to hit a hard stop
+     * Right joystick Y axis (held) -> run climb -- up to extend, down to retract
+     * Left bumper (held)           -> override flywheel only (force flywheel to run)
+     * Right bumper (held)          -> override both flywheel and internals (force flywheel to run, internals after spinup delay)
+     * Right bumper (held)          -> slow mode; scales down drive powers for more precise inputs
+     * POV (0, 180)                 -> turret distance offsets -- 0 to increase r, 180 to decrease
+     * POV (90, 270)                -> turret turntable offsets -- 90 to increase theta, 270 to decrease
      */
     private void configureButtonBindings() {
         driveAButton.whenPressed(new RequestShotCommand(internalSubsystem));
@@ -169,7 +180,6 @@ private void configureButtonBindings() {
         //     () -> turretSubsystem.setDriverOverrideFlywheel(false), 
         //     turretSubsystem
         // ));
-
 
         // Car drive with the left Y axis controlling y power and the right X axis controlling angular
         tankSubsystem.setDefaultCommand(new RunCommand(() -> {

src/main/java/frc/robot/commands/tank/FollowPathCommand.java

@@ -92,10 +92,13 @@ public FollowPathCommand(TankSubsystem tankSubsystem, Pose2d start, List<Transla
                 new TrajectoryConfig(MAX_VEL, MAX_ACCEL)
                     .setReversed(reversed)
                     .setKinematics(KINEMATICS)
-                    .addConstraint(new DifferentialDriveVoltageConstraint(
-                        new SimpleMotorFeedforward(Ks, Kv, Ka), 
-                        KINEMATICS, 
-                        10))
+                    .addConstraint(
+                        new DifferentialDriveVoltageConstraint(
+                            new SimpleMotorFeedforward(Ks, Kv, Ka), 
+                            KINEMATICS, 
+                            10
+                        )
+                    )
             )
         );
     }

src/main/java/frc/robot/jetson/JetsonConnection.java

@@ -87,14 +87,16 @@ class JetsonConnectionRunnable implements Runnable {
         @Override
         public void run() {
             while (true) {
+                /*
                 System.out.println(socket == null 
                     ? "socket is null"
                     : "connected: " + socket.isConnected() + " closed: " + socket.isClosed() + " bound: " + socket.isBound()
                 );
+                */
                 try {
                     // If thread interrupted
                     if (Thread.interrupted()) continue;
-                    System.out.println("not interrupted");
+                    //System.out.println("not interrupted");
 
                     // Check if we need to connect
                     if (stdIn == null || socket == null || socket.isClosed() || !socket.isConnected() || !socket.isBound()) {

src/main/java/frc/robot/subsystems/TurretSubsystem.java

@@ -682,7 +682,7 @@ public TurretMode getMode() {
 
     /**
      * Sets whether the robot is currently being driven by controller input.
-     * @param driving Whethe rthe robot is being driven.
+     * @param driving Whether the robot is being driven.
      */
     public void setDriving(boolean driving) {
         if (!driving) {

src/main/java/frc/robot/subsystems/tank/PoseEstimator.java

@@ -29,13 +29,16 @@ public PoseEstimator(AHRS ahrs, RelativeEncoder leftEncoder, RelativeEncoder rig
         this.rightEncoder.setPosition(0);
 
         // https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-pose_state-estimators.html
-        poseEstimator = new DifferentialDrivePoseEstimator(new Rotation2d(), new Pose2d(),
-                // State measurement standard deviations. X, Y, theta, dist_l, dist_r.
-                new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02),
-                // Odometry measurement standard deviations. Left encoder, right encoder, gyro.
-                new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.02, 0.02, 0.01),
-                // Vision measurement standard deviations. X, Y, and theta.
-                new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1, 0.1, 0.01));
+        poseEstimator = new DifferentialDrivePoseEstimator(
+            new Rotation2d(), 
+            new Pose2d(),
+            // State measurement standard deviations. X, Y, theta, dist_l, dist_r.
+            new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02),
+            // Odometry measurement standard deviations. Left encoder, right encoder, gyro.
+            new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.02, 0.02, 0.01),
+            // Vision measurement standard deviations. X, Y, and theta.
+            new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1, 0.1, 0.01)
+        );
     }
 
     public void update() {
@@ -49,7 +52,6 @@ public void update() {
 
     /**
      * Gets the estimated current position of the robot.
-     * 
      * @return The estimated position of the robot as a Pose2d.
      */
     public Pose2d getPosition() {
@@ -58,7 +60,6 @@ public Pose2d getPosition() {
 
     /**
      * Resets the robot's position to a given Pose2D.
-     * 
      * @param pose The pose to reset the pose estimator to.
      */
     public void setPosition(Pose2d pose) {
@@ -70,9 +71,7 @@ public void setPosition(Pose2d pose) {
     }
 
     /**
-     * Gets the gyro angle given by the NavX AHRS, inverted to be counterclockwise
-     * positive.
-     * 
+     * Gets the gyro angle given by the NavX AHRS, inverted to be counterclockwise positive.
      * @return The robot heading as a Rotation2d.
      */
     private Rotation2d getGyroHeading() {
@@ -81,22 +80,20 @@ private Rotation2d getGyroHeading() {
 
     /**
      * Gets the wheel speeds of the drivetrain.
-     * 
      * @return The drivetrain wheel speeds as a DifferentialDriveWheelSpeeds object.
      */
     private DifferentialDriveWheelSpeeds getWheelSpeeds() {
         return new DifferentialDriveWheelSpeeds(
-                leftEncoder.getVelocity(),
-                rightEncoder.getVelocity());
+            leftEncoder.getVelocity(),
+            rightEncoder.getVelocity()
+        );
     }
 
     /**
      * Gets the last measured wheel speeds of the drivetrain.
-     * 
-     * @return The last measured drivetrain wheel speeds as a
-     *         DifferentialDriveWheelSpeeds object.
+     * @return The last measured drivetrain wheel speeds as a DifferentialDriveWheelSpeeds object.
      */
     public DifferentialDriveWheelSpeeds getLastWheelSpeeds() {
         return this.lastWheelSpeeds;
     }
-}
+}