Merge pull request #34 from Team4388/Autos-and-L4-Placement

Autos and l4 placement

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

@@ -366,7 +366,7 @@ 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(17);
+        public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(16);
         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);
@@ -419,7 +419,7 @@ public final class Constants {
         //Max for elevator = 50%
         
         public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
-        public static final double WAITING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
+        public static final double WAITING_POSITION_ELEVATOR = -9.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;
@@ -437,7 +437,7 @@ public final class Constants {
         public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * 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;
+        public static final double COMPLETLY_TOP_ENDEFECTOR = 0.51 * GEAR_RATIO_ENDEFECTOR;
 
         public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
             .withKP(1)

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

@@ -141,19 +141,26 @@ public class RobotContainer {
 
     private Command AprilLidarAlignL4Left = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
+
         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, 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)
     );
 
@@ -529,7 +536,7 @@ public class RobotContainer {
     //     return autoCommand;
 	// } catch (Exception e) {
 	//     DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
-	    return Commands.none();
+	    return autoCommand;
 	// }
     // return new PathPlannerAuto("Line-up-no-arm");
 	// zach told me to do the below comment
@@ -551,9 +558,9 @@ public class RobotContainer {
 
         autoChooser.onChange((filename) -> {
             autoCommand = new PathPlannerAuto(filename);
-            System.out.println("Robot Auto Changed");
+            System.out.println("Robot Auto Changed " + filename);
         });
-        SmartDashboard.putData(autoChooser);
+        // SmartDashboard.putData(autoChooser);
 
     }
 

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

@@ -178,20 +178,25 @@ public class Elevator extends SubsystemBase {
   public boolean elevatorAtRefrence() {
     // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
     double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
-    boolean atPos = Math.abs(elevatorRefrence - elevatorPosition) <= 0.5;
+    double diffrence = elevatorRefrence - elevatorPosition;
-    if (atPos) {
-      SmartDashboard.putNumber("Elevator Refrence", elevatorRefrence);
-      SmartDashboard.putNumber("Elevator Pos", elevatorPosition);
-    }
 
-    return atPos;
+    boolean headedUp = diffrence < 0;
+    boolean forwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0;
+    boolean reverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0;
+
+    return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
   }
-
+  
   public boolean endefectorAtRefrence() {
     // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
     double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble();
+    double diffrence = endefectorRefrence - endefectorPosition;
 
-    return Math.abs(endefectorRefrence - endefectorPosition) <= 0.2;
+    boolean headedUp = diffrence < 0;
+    boolean forwardLimit = endefectorMotor.getForwardLimit().asSupplier().get().value == 0;
+    boolean reverseLimit = endefectorMotor.getReverseLimit().asSupplier().get().value == 0;
+
+    return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
   }
   // public void driveElevatorStick(Translation2d stick) {
   //   if (stick.getNorm() > 0.05) {
@@ -210,7 +215,7 @@ public class Elevator extends SubsystemBase {
   // }
   
   private void periodicReady() {
-    if (elevatorAtRefrence())
+    if (elevatorMotor.getForwardLimit().asSupplier().get().value == 0)
       transitionState(CoordinationState.Hovering);
   }
 
@@ -227,7 +232,7 @@ public class Elevator extends SubsystemBase {
     SmartDashboard.putString("State", currentState.toString());
 
     if (currentState == CoordinationState.Waiting) {
-      // periodicWaiting();
+      periodicWaiting();
     } else if (currentState == CoordinationState.WatingBeamTriped) {
       // periodicWaitingTripped();
     } else if (currentState == CoordinationState.Ready) {