2/17 Full changes

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

@@ -404,11 +404,11 @@ public final class Constants {
         public static final double GEAR_RATIO_ELEVATOR = -9.0;
 
         public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
-        public static final double WAITING_POSITION_ELEVATOR = 2 * GEAR_RATIO_ELEVATOR; // TODO: find 2-4 in off the pipe
+        public static final double WAITING_POSITION_ELEVATOR = (2 / 3.d) * GEAR_RATIO_ELEVATOR; // TODO: find 2-4 in off the pipe
         public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = 2 * GEAR_RATIO_ELEVATOR; // TODO: find on the pipe
         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 GEAR_RATIO_ENDEFECTOR = -100.0;
 
         public static final double COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
         public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
@@ -416,7 +416,7 @@ public final class Constants {
         public static final double COMPLETLY_TOP_ENDEFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
 
         public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
-            .withKP(.1)
+            .withKP(1)
             .withKI(0)
             .withKD(0);
         

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

@@ -175,6 +175,12 @@ public class RobotContainer {
         }, m_robotSwerveDrive)
         .withName("SwerveDrive DefaultCommand"));
         m_robotSwerveDrive.setToSlow();
+        // m_robotElevator.setDefaultCommand(new RunCommand(() -> {
+        //     m_robotElevator.driveElevatorStick(m_operatorXbox.getRight());
+        // }, m_robotElevator)
+        // .withName("Elevator"));
+
+
         makeAutoChooser();
         SmartDashboard.putData("Auto Chooser", autoChooser);
         // this.subsystems.add(m_robotSwerveDrive);
@@ -225,19 +231,19 @@ public class RobotContainer {
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
 
         // ! /* Speed */
-        new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
+        new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
             .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
         
-        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
+        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
 
-        new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) // final
+        new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
             .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.activateLuigiMode()));
         
-        new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) // final
+        new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
             .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.deactivateLuigiMode()));
 
-        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
+        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
 
         new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)

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

@@ -5,10 +5,12 @@
 package frc4388.robot.subsystems;
 
 import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.controls.PositionDutyCycle;
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -58,8 +60,8 @@ public class Elevator extends SubsystemBase {
   //PID methods
 
   private void PIDPosition(TalonFX motor, double position) {
-    var request = new PositionVoltage(position);
+    var request = new PositionDutyCycle(position);
-    elevatorMotor.setControl(request);
+    motor.setControl(request);
   }
 
   public void elevatorStop() {
@@ -82,6 +84,7 @@ public class Elevator extends SubsystemBase {
       case WatingBeamTriped: {
         PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
         PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
+        armShuffle();
         break;
       }
 
@@ -106,6 +109,12 @@ public class Elevator extends SubsystemBase {
 
   }
 
+  // public void driveElevatorStick(Translation2d stick) {
+  //   if (stick.getNorm() > 0.05) {
+  //     elevatorMotor.set(stick.getY());
+  //   }
+  // }
+
   private void periodicWaiting() {
     if (basinBeamBreak.get()) transitionState(CoordinationState.WatingBeamTriped);
   }
@@ -122,14 +131,21 @@ public class Elevator extends SubsystemBase {
   public void periodic() {
     // This method will be called once per scheduler run
     SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
-    SmartDashboard.putNumber("endefector", basinBeamBreak.get() ? 1 : 0);
+    SmartDashboard.putNumber("endefector", endefectorLimitSwitch.get() ? 1 : 0);
-    if (currentState == CoordinationState.Waiting) {
+
-      periodicWaiting();
+    // if (currentState == CoordinationState.Waiting) {
-    } else if (currentState == CoordinationState.WatingBeamTriped) {
+    //   periodicWaiting();
-      periodicWaitingTripped();
+    // } else if (currentState == CoordinationState.WatingBeamTriped) {
-    }
+    //   periodicWaitingTripped();
+    // }
     // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
     //   periodicScoring();
     // }
   }
+
+  public void armShuffle(){
+    if(!basinBeamBreak.get()){
+      //shuffle the coral with the arm until coral hits beam break
+    }
+  }
 }