Optimization and improvements

src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path

@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 4.39747159090909,
+        "x": 2.4125846006173957,
-        "y": 1.3027556818181814
+        "y": 1.7771562346874141
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.3048863636363635,
+        "x": 3.5997443181818185,
-        "y": 2.5292613636363637
+        "y": 2.6888068181818174
       },
       "prevControl": {
-        "x": 5.893210227272728,
+        "x": 2.6837095087024796,
-        "y": 1.9808238636363626
+        "y": 1.9835311238550895
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,7 +42,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 114.22774531795413
+    "rotation": 56.633633998940475
   },
   "reversed": false,
   "folder": null,

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

@@ -69,7 +69,7 @@ public final class Constants {
     
     public static final class SwerveDriveConstants {
 
-        public static final double MAX_ROT_SPEED        = 3.5;
+        public static final double MAX_ROT_SPEED        = Math.PI * 2;
         public static final double AUTO_MAX_ROT_SPEED = 1.5;
         public static final double MIN_ROT_SPEED        = 1.0;
         public static       double ROTATION_SPEED       = MAX_ROT_SPEED;
@@ -329,7 +329,7 @@ public final class Constants {
     }
 
     public static final class AutoConstants {
-        public static final Gains XY_GAINS = new Gains(3,0.01,0.0);
+        public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
         public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
 
         public static final Trim X_OFFSET_TRIM =        new Trim("X Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
@@ -340,13 +340,16 @@ public final class Constants {
         public static final double XY_TOLERANCE = 0.07; // Meters
         public static final double ROT_TOLERANCE = 5; // Degrees
 
+        public static final double MIN_XY_PID_OUTPUT = 0.0;
+        public static final double MIN_ROT_PID_OUTPUT = 0.0;
+                
         // X is tangent to reef side
         // Y is normal to reef side
         public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // This is from the field
         public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16);
 
         public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
-        public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
+        public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
         
         public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
         public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
@@ -366,15 +369,15 @@ public final class Constants {
         public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT";
 
         public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
-        public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
+        public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547+(1*-1)), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
         
-        public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters
+        public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters
 
         // Photonvision thing
         // The standard deviations of our vision estimated poses, which affect correction rate
         // X, Y, Theta
         // https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2
-        public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
+        public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(3, 3, 4);
         public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
     }
 
@@ -424,6 +427,8 @@ public final class Constants {
         public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
         public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
 
+        // public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
+        
         public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
         public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND
         
@@ -432,6 +437,7 @@ public final class Constants {
         
         public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
         public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe
+        public static final double HOVERING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
         public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe
         public static final double SCORING_THREE_ELEVATOR = -9.25;
         public static final double DEALGAE_L2_ELEVATOR = -23.5;

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

@@ -149,7 +149,11 @@ public class RobotContainer {
         new MoveForTimeCommand(m_robotSwerveDrive, 
         new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
 
-        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
+        new ConditionalCommand(
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
+             () -> m_robotElevator.hasCoral()),
+
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 
@@ -177,11 +181,17 @@ public class RobotContainer {
 
         new waitEndefectorRefrence(m_robotElevator),
 
+        
         new MoveForTimeCommand(m_robotSwerveDrive, 
             new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
 
+        new ConditionalCommand(
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
+                () -> m_robotElevator.hasCoral()),
+
+
 
-        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 
@@ -295,8 +305,10 @@ public class RobotContainer {
     );
 
     private Command thrustIntake = new SequentialCommandGroup(
-        new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,-1), new Translation2d(), 100, true),
+        new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod(), m_robotSwerveDrive),
-        new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)
+        new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,-1), new Translation2d(), 300, true),
+        new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive),
+        new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod(), m_robotSwerveDrive)
     );
     
     private Boolean operatorManualMode = false;
@@ -310,7 +322,7 @@ public class RobotContainer {
         NamedCommands.registerCommand("align-feed", new SequentialCommandGroup(
             new MoveForTimeCommand(m_robotSwerveDrive, 
                 new Translation2d(0, 1), 
-                new Translation2d(), 200, true
+                new Translation2d(), 400, true
         ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
         
         NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
@@ -428,6 +440,10 @@ public class RobotContainer {
         new Trigger(() -> getDeadbandedDriverController().getPOV() == 270 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
             .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
         
+        new Trigger(() ->getDeadbandedDriverController().getRightTriggerAxis() > 0.8)
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod()))
+            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod()));
+        
         // While Left Trigger NOT Pressed: Fine Alignment
         new Trigger(() -> getDeadbandedDriverController().getPOV() != -1 && !(getDeadbandedDriverController().getLeftTriggerAxis() > 0.8))
             .whileTrue(new RunCommand(
@@ -436,7 +452,7 @@ public class RobotContainer {
                         1, 
                         Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV())
                     ), 
-                    getDeadbandedDriverController().getRight(), 0.15
+                    getDeadbandedDriverController().getRight(), 0.30
                 ), m_robotSwerveDrive))
             .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
         
@@ -511,10 +527,10 @@ public class RobotContainer {
             .onTrue (new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed), m_robotElevator))
             .onFalse(new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go), m_robotElevator));
         
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
             .onTrue(new InstantCommand(() -> {operatorManualMode = !operatorManualMode;}));
         
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking()));
 
         // Auto Scoring 

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

@@ -1,5 +1,7 @@
 package frc4388.robot.commands;
 
+import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
+
 import java.util.Optional;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -74,6 +76,10 @@ public class GotoLastApril extends Command {
     double yerr;
     double roterr;
 
+    double xoutput;
+    double youtput;
+    double rotoutput;
+
     @Override
     public void execute() {
         xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
@@ -100,9 +106,9 @@ public class GotoLastApril extends Command {
         // SmartDashboard.putNumber("PID X Error", xerr);
         // SmartDashboard.putNumber("PID Y Error", yerr);
 
-        double xoutput = xPID.update(xerr);
+        xoutput = xPID.update(xerr);
-        double youtput = yPID.update(yerr);
+        youtput = yPID.update(yerr);
-        double rotoutput = rotPID.update(roterr);
+        rotoutput = rotPID.update(roterr);
 
         xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
         youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
@@ -133,7 +139,10 @@ public class GotoLastApril extends Command {
 
     @Override
     public final boolean isFinished() {
-        boolean finished = (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE);
+        boolean finished = (
+            (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
+            (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
+            (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE || Math.abs(rotoutput) <= AutoConstants.MIN_ROT_PID_OUTPUT));
         // System.out.println(finished);
 
         if(finished)

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

@@ -49,6 +49,7 @@ public class Elevator extends Subsystem {
 
   private DigitalInput basinBeamBreak;
   private DigitalInput endeffectorLimitSwitch;
+  // private DigitalInput intakeIR;
 
   public enum CoordinationState {
     Waiting, // for coral into the though
@@ -69,12 +70,14 @@ public class Elevator extends Subsystem {
   private CoordinationState currentState;
 
   public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
+  // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) {
     elevatorMotor = elevatorTalonFX;
     endeffectorMotor = endeffectorTalonFX;
     this.led = led;
 
     this.basinBeamBreak = basinLimitSwitch;
     this.endeffectorLimitSwitch = endeffectorLimitSwitch;
+    // this.intakeIR = intakeDigitalInput;
 
     elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
     endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
@@ -129,14 +132,14 @@ public class Elevator extends Subsystem {
       }
 
       case Hovering: {
-        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
+        PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR);
         PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
         led.setMode(LEDConstants.READY_PATTERN);
         break;
       }
 
       case L2Score: {
-        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
         led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
         break;
@@ -240,8 +243,8 @@ public class Elevator extends Subsystem {
   private void periodicWaiting() {
     if (!basinBeamBreak.get()) 
       transitionState(CoordinationState.Ready);
-    if(!endeffectorLimitSwitch.get())
+    // if(!endeffectorLimitSwitch.get())
-      transitionState(CoordinationState.Hovering);
+    //   transitionState(CoordinationState.Hovering);
   }
 
   // private void periodicWaitingTripped() {
@@ -253,7 +256,7 @@ public class Elevator extends Subsystem {
     if (elevatorAtReference() && !endeffectorLimitSwitch.get())
       transitionState(CoordinationState.Hovering);
     if(elevatorAtReference() && endeffectorLimitSwitch.get())
-      transitionState(CoordinationState.Waiting);
+      transitionState(CoordinationState.Hovering);
   }
 
   private void periodicScoring() {

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

@@ -326,7 +326,7 @@ public class SwerveDrive extends Subsystem {
                 rotTarget += deltaAngle;
             }
             
-            swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time);
+            swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);
         }
 
         // if(e.isPresent())
@@ -392,6 +392,11 @@ public class SwerveDrive extends Subsystem {
         setToSlow();
     }
 
+    public void startTurboPeriod() {
+        tmp_gear_index = gear_index;
+        setToTurbo();
+    }
+
     public void endSlowPeriod() {
         setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]);
         gear_index = tmp_gear_index;

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

@@ -48,7 +48,7 @@ public class Vision extends Subsystem {
 
     private boolean isTagDetected = false;
     private boolean isTagProcessed = false;
-    private Pose2d lastVisionPose = new Pose2d();
+    public Pose2d lastVisionPose = new Pose2d();
     private Pose2d lastPhysOdomPose = new Pose2d();
 
     private Matrix<N3, N1> curStdDevs;
@@ -265,7 +265,8 @@ public class Vision extends Subsystem {
 
     public Pose2d getPose2d() {
         if(isTagDetected && isTagProcessed)
-            return lastVisionPose;
+            // return lastVisionPose;
+            return lastPhysOdomPose;
         else
             return lastPhysOdomPose;
     }