Add codes

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

@@ -242,7 +242,7 @@ public final class Constants {
             public static final int SECONDS_TO_MICROS = 1000000;
                     
             public static final double XY_TOLERANCE = 0.07; // Meters
-            public static final double ROT_TOLERANCE = 1; // Degrees
+            public static final double ROT_TOLERANCE = 5; // Degrees
                     
             // public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
             // public static final Pose2d targetpos = 
@@ -365,9 +365,11 @@ public final class Constants {
         ;;;;;;;;;;;;;;;;;;;;;;;;;;;;
         // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right
 
-        public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20);
+        public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(17);
         public static final double L4_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
         public static final double L4_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
+        public static final double L3_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
+        public static final double L3_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
 
 
 
@@ -413,6 +415,7 @@ public final class Constants {
         public static final double WAITING_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;
         public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position
         
         public static final double GEAR_RATIO_ENDEFECTOR = -100.0;
@@ -421,7 +424,7 @@ public final class Constants {
 
         public static final double COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
         public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
-        public static final double SCORING_THREE_ENDEFECTOR = 0.375 * GEAR_RATIO_ENDEFECTOR;
+        public static final double PRIMED_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
         public static final double SCORING_FOUR_ENDEFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
         public static final double COMPLETLY_TOP_ENDEFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
 

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

@@ -16,6 +16,7 @@ import java.util.List;
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -115,19 +116,74 @@ public class RobotContainer {
     // private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
     private Command AprilLidarAlign = new SequentialCommandGroup(
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
         // new InstantCommand(() -> System.out.println("Soup")),
         // new WaitCommand(1),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1 - Units.inchesToMeters(0), Side.RIGHT),
         new LidarAlign(m_robotSwerveDrive, m_lidar),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT),
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
         new Translation2d(0,1), new Translation2d(), 500, true),
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
     );
+
+    private Command AprilLidarAlignLeft = new SequentialCommandGroup(
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
+        // new InstantCommand(() -> System.out.println("Soup")),
+        // new WaitCommand(1),
+        new waitEndefectorRefrence(m_robotElevator),
+        new waitElevatorRefrence(m_robotElevator),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT),
+        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
+        new waitEndefectorRefrence(m_robotElevator),
+        new MoveForTimeCommand(m_robotSwerveDrive, 
+        new Translation2d(0,1), new Translation2d(), 500, true),
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
+    );
+
+    private Command AprilLidarAlignL3Left = new SequentialCommandGroup(
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT),
+        // new InstantCommand(() -> System.out.println("Soup")),
+        // new WaitCommand(1),
+        new waitEndefectorRefrence(m_robotElevator),
+        new waitElevatorRefrence(m_robotElevator),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT),
+        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
+        new waitEndefectorRefrence(m_robotElevator),
+        new waitElevatorRefrence(m_robotElevator),
+        // new MoveForTimeCommand(m_robotSwerveDrive, 
+        // new Translation2d(0,1), new Translation2d(), 500, true),
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
+    );
+
+    private Command AprilLidarAlignL3 = new SequentialCommandGroup(
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
+
+        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
+
+        new waitEndefectorRefrence(m_robotElevator),
+        new waitElevatorRefrence(m_robotElevator),
+
+        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.RIGHT),
+        
+        new LidarAlign(m_robotSwerveDrive, m_lidar),
+
+        // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
+
+        new waitEndefectorRefrence(m_robotElevator),
+        new waitElevatorRefrence(m_robotElevator),
+        // new MoveForTimeCommand(m_robotSwerveDrive, 
+        // new Translation2d(0,1), new Translation2d(), 500, true),
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
+    );
+
     private Command AprilLidarLeft = new SequentialCommandGroup(
         AprilLidarAlign.asProxy(),
         new LidarAlign(m_robotSwerveDrive, m_lidar)
@@ -265,12 +321,22 @@ public class RobotContainer {
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
             
         
-        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
+            .onTrue(AprilLidarAlignLeft);
+        
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
             .onTrue(AprilLidarAlign);
 
         new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
             .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive));
           
+
+        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+            .onTrue(AprilLidarAlign);
+        
+        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+            .onTrue(AprilLidarAlign);
+
         // ?  /* Operator Buttons */
 
         new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
@@ -281,17 +347,17 @@ public class RobotContainer {
             .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); 
 
         
-        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
+        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator));
         
-        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
+        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator));
         
-        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator));
+            .onTrue(AprilLidarAlignL3Left);
-
+        
-        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator));
+            .onTrue(AprilLidarAlignL3);
 
         DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator));

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

@@ -78,6 +78,8 @@ public class GotoLastApril extends Command {
         youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
         rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
 
+        // System.out.println(xerr + ", " + yerr + ", " + roterr + ", " + rotoutput);
+
         Translation2d leftStick = new Translation2d(
             Math.max(Math.min(-youtput, 1), -1),
             Math.max(Math.min(-xoutput, 1), -1)

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

@@ -39,7 +39,7 @@ public class LidarAlign extends Command {
     this.currentFinderTick = 0;
     this.speed = 0.4; // TODO: find good speed for this
     this.foundReef = false;
-    this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
+    this.headedRight = (GotoLastApril.tagRelativeXError < 0);
   }
 
 
@@ -52,7 +52,7 @@ public class LidarAlign extends Command {
       return;
     }
 
-    if (currentFinderTick > 100) { //arbutrary threshhold for now.
+    if (currentFinderTick > 40) { //arbutrary threshhold for now.
       headedRight = !headedRight;
       currentFinderTick *= -1;
     }

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

@@ -106,12 +106,6 @@ public class Elevator extends SubsystemBase {
         break;
       }
 
-      case ScoringThree: {
-        PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR);
-        PIDPosition(endefectorMotor, ElevatorConstants.SCORING_THREE_ENDEFECTOR);
-        break;
-      }
-
       case PrimedFour: {
         PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
         PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
@@ -123,6 +117,22 @@ public class Elevator extends SubsystemBase {
         PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR);
         break;
       }
+
+      case PrimedThree: {
+        PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR);
+        PIDPosition(endefectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFECTOR);
+        break;
+      }
+      
+      case ScoringThree: {
+        PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR);
+        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
+        break;
+      }
+
+      default: {
+        assert false;
+      }
     }
 
   }
@@ -148,7 +158,7 @@ public class Elevator extends SubsystemBase {
 
   private void periodicWaiting() {
     if (!basinBeamBreak.get()) 
-      transitionState(CoordinationState.WatingBeamTriped);
+      transitionState(CoordinationState.Ready);
   }
 
   private void periodicWaitingTripped() {
@@ -157,8 +167,8 @@ public class Elevator extends SubsystemBase {
   }
   
   private void periodicReady() {
-    if (elevatorAtRefrence())
+    // if (elevatorAtRefrence())
-      transitionState(CoordinationState.Hovering);
+    //   transitionState(CoordinationState.Hovering);
   }
 
   private void periodicScoring() {
@@ -173,11 +183,11 @@ public class Elevator extends SubsystemBase {
     SmartDashboard.putNumber("endefector", endefectorLimitSwitch.get() ? 1 : 0);
 
     if (currentState == CoordinationState.Waiting) {
-      periodicWaiting();
+      // periodicWaiting();
     } else if (currentState == CoordinationState.WatingBeamTriped) {
-      periodicWaitingTripped();
+      // periodicWaitingTripped();
     } else if (currentState == CoordinationState.Ready) {
-      periodicReady();
+      // periodicReady();
     }
     // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
     //   periodicScoring();