l2 baby

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

@@ -234,7 +234,10 @@ public final class Constants {
             public static final Gains XY_GAINS = new Gains(3,0.01,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.05, 0);
+            public static final Trim X_OFFSET_TRIM =        new Trim("X Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
+            public static final Trim Y_OFFSET_TRIM =        new Trim("Y Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
+            public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0);
+            public static final Trim ARM_OFFSET_TRIM =      new Trim("ARM Offset Trim",      -ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR, 1, 0);
 
             public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
             public static final int LIDAR_DIO_CHANNEL = 7;
@@ -361,8 +364,6 @@ public final class Constants {
 
         public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5);
 
-
-        ;;;;;;;;;;;;;;;;;;;;;;;;;;;;
         // 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);
@@ -418,12 +419,15 @@ public final class Constants {
         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 DEALGAE_L3_ELEVATOR = -33.75;
         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;
         public static final double ENDEFECTOR_DRIVE_SLOW = -0.08;
         //Max for endefector = 60%
 
+        public static final double L2_SCORE_ENDEFECTOR = -19;
+
         public static final double COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
         public static final double DEALGAE_L2_EENDEFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR;
         public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;

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

@@ -314,11 +314,20 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
 
-        new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot()));
 
-        new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
+        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
+            .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepUp()));
+
+        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
+            .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown()));
+
+
+        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90)
+            .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp()));
+
+        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270)
+            .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepDown()));
+
             
         
         new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
@@ -360,6 +369,23 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
             .onTrue(AprilLidarAlignL3);
 
+
+        //Trims
+        new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
+            .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp()));
+
+        new Trigger(() -> getDeadbandedDriverController().getPOV() == 180)
+            .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown()));
+
+
+        new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
+            .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
+
+        new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
+            .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
+
+        
+        // Lower coral removal
         DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
             .onTrue(new SequentialCommandGroup(
                 new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
@@ -372,6 +398,19 @@ public class RobotContainer {
                     new Translation2d(0,1), new Translation2d(), 500, true)
             ));
         
+        // Upper coral removal
+        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON)
+            .onTrue(new SequentialCommandGroup(
+                new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
+                new waitEndefectorRefrence(m_robotElevator),
+                new waitElevatorRefrence(m_robotElevator),
+                new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_ALGAE_REMOVAL - Units.inchesToMeters(1), Side.LEFT),
+                new LidarAlign(m_robotSwerveDrive, m_lidar),
+                new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
+                new MoveForTimeCommand(m_robotSwerveDrive, 
+                    new Translation2d(0,1), new Translation2d(), 500, true)
+            ));
+        
             
         // ? /* Programer Buttons (Controller 3)*/
 

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

@@ -3,6 +3,7 @@ package frc4388.robot.commands;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
@@ -54,7 +55,9 @@ public class GotoLastApril extends Command {
     public void initialize() {
         xPID.initialize();
         yPID.initialize();
-        this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side, AutoConstants.X_OFFSET_TRIM.get(), distance);
+        this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side, 
+        Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()), 
+        distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get()));
     }
     
     double xerr;

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

@@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.ElevatorConstants;
+import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
 
 public class Elevator extends SubsystemBase {
   /** Creates a new Elevator. */
@@ -28,6 +29,9 @@ public class Elevator extends SubsystemBase {
   private long wait = 0;
   private long maxWait = 1000;
 
+  private double elevatorRefrence = 0;
+  private double endefectorRefrence = 0;
+
   private DigitalInput basinBeamBreak;
   private DigitalInput endefectorLimitSwitch;
 
@@ -36,6 +40,7 @@ public class Elevator extends SubsystemBase {
     WatingBeamTriped, //once the beam break trips
     Ready, // Has coral in endefector
     Hovering, // Has coral in endefector
+    L2Score,
     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
@@ -66,6 +71,9 @@ public class Elevator extends SubsystemBase {
   //PID methods
 
   private void PIDPosition(TalonFX motor, double position) {
+    if (motor == elevatorMotor) elevatorRefrence = position;
+    else endefectorRefrence = position;
+
     var request = new PositionDutyCycle(position);
     motor.setControl(request);
   }
@@ -106,39 +114,57 @@ public class Elevator extends SubsystemBase {
         break;
       }
 
+      case L2Score: {
+        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(endefectorMotor, ElevatorConstants.L2_SCORE_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        break;
+      }
+      
       case PrimedFour: {
-        PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
+        PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
         break;
       }
 
       case ScoringFour: {
-        PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
+        PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR);
+        PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
         break;
       }
 
       case PrimedThree: {
-        PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR);
+        PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endefectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFECTOR);
         break;
       }
       
       case ScoringThree: {
-        PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR);
+        PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
+        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
         break;
       }
 
       case BallRemoverL2Primed: {
-        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
+        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR);
         break;
       }
 
       case BallRemoverL2Go: {
-        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
+        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR);
+        PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        break;
+      }
+
+      case BallRemoverL3Primed: {
+        PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR);
+        break;
+      }
+
+      case BallRemoverL3Go: {
+        PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
         break;
       }
 
@@ -150,17 +176,22 @@ public class Elevator extends SubsystemBase {
   }
 
   public boolean elevatorAtRefrence() {
-    double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
+    // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
     double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
+    boolean atPos = Math.abs(elevatorRefrence - elevatorPosition) <= 0.1;
+    if (atPos) {
+      SmartDashboard.putNumber("Elevator Refrence", elevatorRefrence);
+      SmartDashboard.putNumber("Elevator Pos", elevatorPosition);
+    }
 
-    return Math.abs(elevatorRefrence - elevatorPosition) <= 0.1;
+    return atPos;
   }
 
   public boolean endefectorAtRefrence() {
-    double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
+    // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
-    double elevatorPosition = endefectorMotor.getPosition().getValueAsDouble();
+    double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble();
 
-    return Math.abs(elevatorRefrence - elevatorPosition) <= 0.2;
+    return Math.abs(endefectorRefrence - endefectorPosition) <= 0.2;
   }
   // public void driveElevatorStick(Translation2d stick) {
   //   if (stick.getNorm() > 0.05) {
@@ -173,14 +204,14 @@ public class Elevator extends SubsystemBase {
       transitionState(CoordinationState.Ready);
   }
 
-  private void periodicWaitingTripped() {
+  // private void periodicWaitingTripped() {
-    if (!basinBeamBreak.get() && System.currentTimeMillis() > wait) 
+  //   if (!basinBeamBreak.get() && System.currentTimeMillis() > wait) 
-      transitionState(CoordinationState.Ready);
+  //     transitionState(CoordinationState.Ready);
-  }
+  // }
   
   private void periodicReady() {
-    // if (elevatorAtRefrence())
+    if (elevatorAtRefrence())
-    //   transitionState(CoordinationState.Hovering);
+      transitionState(CoordinationState.Hovering);
   }
 
   private void periodicScoring() {
@@ -193,13 +224,14 @@ public class Elevator extends SubsystemBase {
     // This method will be called once per scheduler run
     SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
     SmartDashboard.putNumber("endefector", endefectorLimitSwitch.get() ? 1 : 0);
+    SmartDashboard.putString("State", currentState.toString());
 
     if (currentState == CoordinationState.Waiting) {
-      // periodicWaiting();
+      periodicWaiting();
     } else if (currentState == CoordinationState.WatingBeamTriped) {
       // periodicWaitingTripped();
     } else if (currentState == CoordinationState.Ready) {
-      // periodicReady();
+      periodicReady();
     }
     // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
     //   periodicScoring();