March 5th driver practice

src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto

@@ -22,12 +22,6 @@
             "name": "place-coral-left-l4"
           }
         },
-        {
-          "type": "named",
-          "data": {
-            "name": "stop"
-          }
-        },
         {
           "type": "path",
           "data": {

src/main/deploy/pathplanner/paths/Cage 5 to Reef.path

@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.000189393939394,
+        "x": 5.959532828282828,
-        "y": 1.6364267676767676
+        "y": 1.5754419191919191
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.47165404040404,
+        "x": 5.349684343434343,
-        "y": 2.1141414141414145
+        "y": 2.2869318181818175
       },
       "prevControl": {
-        "x": 5.86940340909091,
+        "x": 5.747433712121213,
-        "y": 1.7593308080808083
+        "y": 1.9321212121212112
       },
       "nextControl": null,
       "isLocked": false,
@@ -30,7 +30,18 @@
   ],
   "rotationTargets": [],
   "constraintZones": [],
-  "pointTowardsZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.8,
+        "y": 3.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 1.0,
+      "name": "Point Towards Zone"
+    }
+  ],
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 3.0,

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

@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 5.0,
+    "maxVelocity": 3.0,
-    "maxAcceleration": 5.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 58.10920819815426
   },
-  "useDefaultConstraints": false
+  "useDefaultConstraints": true
 }

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

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.1010732323232322,
+        "x": 1.286335227272727,
-        "y": 1.00625
+        "y": 1.2828125
       },
       "prevControl": {
-        "x": 1.786268939393939,
+        "x": 1.9715309343434337,
-        "y": 2.2439551767676766
+        "y": 2.5205176767676765
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 5.0,
+    "maxVelocity": 3.0,
-    "maxAcceleration": 5.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 121.42956561483854
   },
-  "useDefaultConstraints": false
+  "useDefaultConstraints": true
 }

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

@@ -427,10 +427,11 @@ public final class Constants {
         public static final double ENDEFECTOR_DRIVE_SLOW = -0.08;
         //Max for endefector = 60%
         public static final double L2_SCORE_ELEVATOR = -5;
+        public static final double L2_LEAVE_ELEVATOR = -11;
 
         public static final double L2_SCORE_ENDEFFECTOR = -19;
 
-        public static final double COMPLETLY_DOWN_ENDEFFECTOR = 9001;
+        public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
         public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR;
         public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
         public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;

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

@@ -274,6 +274,8 @@ public class RobotContainer {
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
+        new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2ScoreLeave);}, m_robotElevator),
+        new waitElevatorRefrence(m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
         new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
@@ -293,6 +295,8 @@ public class RobotContainer {
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
+        new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2ScoreLeave);}, m_robotElevator),
+        new waitElevatorRefrence(m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
         new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
@@ -326,7 +330,7 @@ public class RobotContainer {
             new Translation2d(1,0), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME / 2, true),
         new MoveForTimeCommand(m_robotSwerveDrive, 
             new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME * 2, true),
-        // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
+        new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 
@@ -354,7 +358,7 @@ public class RobotContainer {
         // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
             new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME, true),
-        // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
+        new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 

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

@@ -48,6 +48,9 @@ public class Elevator extends Subsystem {
 
   private boolean disableAutoIntake = false;
 
+  private boolean seededZeroEndefector = false;
+  private boolean seededZeroElevator = false;
+
   private DigitalInput basinBeamBreak;
   private DigitalInput endeffectorLimitSwitch;
   private DigitalInput intakeIR;
@@ -58,6 +61,7 @@ public class Elevator extends Subsystem {
     Ready, // Has coral in endefector
     Hovering, // Has coral in endefector
     L2Score,
+    L2ScoreLeave,
     PrimedThree, // Arm and elevator Waiting to score in the level 3 position
     ScoringThree, // Arm and elevator in the level three position
     PrimedFour, // Arm and elevator Waiting to score in the level 4 position
@@ -116,7 +120,7 @@ public class Elevator extends Subsystem {
       case Waiting: {
         wait = System.currentTimeMillis() + maxWait;
         PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
-        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0));
         led.setMode(LEDConstants.WAITING_PATTERN);
         break;
       }
@@ -129,7 +133,7 @@ public class Elevator extends Subsystem {
       }
 
       case Ready: {
-        PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR);
+        PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0));
         PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
         led.setMode(LEDConstants.DOWN_PATTERN);
         break;
@@ -149,6 +153,13 @@ public class Elevator extends Subsystem {
         break;
       }
 
+      case L2ScoreLeave: {
+        PIDPosition(elevatorMotor, ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+      
       case PrimedFour: {
         PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR);
@@ -313,6 +324,16 @@ public class Elevator extends Subsystem {
     SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0);
     SmartDashboard.putString("State", currentState.toString());
 
+    if (!seededZeroEndefector && endeffectorMotor.getForwardLimit().asSupplier().get().value == 0) {
+      endeffectorMotor.setPosition(0);
+      seededZeroEndefector = !seededZeroEndefector;
+    }
+
+    if (!seededZeroElevator && elevatorMotor.getForwardLimit().asSupplier().get().value == 0) {
+      elevatorMotor.setPosition(0);
+      seededZeroElevator = !seededZeroElevator;
+    }
+    
     if (disableAutoIntake) return;
 
     if (currentState == CoordinationState.Waiting) {

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

@@ -9,6 +9,7 @@ import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 
+import java.time.Instant;
 import java.util.List;
 import java.util.Optional;
 
@@ -124,6 +125,8 @@ public class Vision extends Subsystem {
         rotations -= 180;
     }
 
+    private Instant lastVisionTime = null;
+
 
     private void update() {
         isTagProcessed = false;
@@ -176,20 +179,27 @@ public class Vision extends Subsystem {
         lastLatency = latency / cams;
 
         if(isTagProcessed){
+            Instant now = Instant.now();
 
-            // double lastAngle = lastVisionPose.getRotation().getDegrees();
             double curAngle = Yaw/cams;
 
-            // if(lastAngle - curAngle >= 90){
+            // if(lastVisionTime != null && Math.abs(now.getEpochSecond() - lastVisionTime.getEpochSecond()) > 1){            
-            //     subtractRotation();
+            //     double lastAngle = lastVisionPose.getRotation().getDegrees();
-            // }else if(lastAngle - curAngle <= -90){
+
+            //     if(lastAngle - curAngle >= 80){
             //         addRotation();
+            //     }else if(lastAngle - curAngle <= -80){
+            //         subtractRotation();
             //     }
+            // }
+
+
 
             // SmartDashboard.putNumber("curAngle", curAngle);
             // SmartDashboard.putNumber("Rotations", rotations);
             
             lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle));
+            lastVisionTime = now;
         }
     }