Elevator+trap testing to main (#112)

Patribots4738 · Feb 2, 2024 · cb45de7 · cb45de7
2 parents 612e9fa + 170dd1b
commit cb45de7
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 16 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -134,7 +134,11 @@ private void configureOperatorBindings() {
         operator.povUp().toggleOnTrue(climb.povUpCommand(swerve::getPose));
 
         operator.povDown().onTrue(climb.toBottomCommand());
-
+
+        operator.povLeft().onTrue(elevator.toBottomCommand());
+
+        operator.povRight().onTrue(pieceControl.placeTrapCommand());
+
         operator.leftBumper()
             .and(operator.rightBumper())
             .onTrue(pieceControl.noteToShoot());

diff --git a/src/main/java/frc/robot/commands/PieceControl.java b/src/main/java/frc/robot/commands/PieceControl.java
@@ -101,8 +101,11 @@ public Command noteToTarget(BooleanSupplier toAmp) {
         return shoot;
     }
 
-    public Command goHomeElevator() {
-        return this.elevator.setPositionCommand(TrapConstants.RESET_POS);
+    public Command placeTrapCommand() {
+        return Commands.sequence(
+                elevator.toTopCommand(),
+                claw.placeCommand(),
+                Commands.waitSeconds(2));
     }
 
 }
diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java
@@ -30,11 +30,7 @@ public void configMotors() {
         elevator.setPeriodicFramePeriod(PeriodicFrame.kStatus3, 65535);
         elevator.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 65535);
         elevator.getEncoder().setPositionConversionFactor(TrapConstants.ELEVATOR_POSITION_CONVERSION_FACTOR);
-        elevator.setPID(
-                TrapConstants.TRAP_P,
-                TrapConstants.TRAP_I,
-                TrapConstants.TRAP_D,
-                TrapConstants.TRAP_ELEVATOR_MIN_OUTPUT, TrapConstants.TRAP_ELEVATOR_MAX_OUTPUT);
+        elevator.setPID(TrapConstants.TRAP_PID);
     }
 
     @Override
@@ -77,6 +73,22 @@ public Command setPositionCommand(double pos) {
                 .andThen(Commands.waitUntil(this.isAtTargetPosition()));
     }
 
+    private void toTop() {
+        this.setPosition(TrapConstants.TRAP_PLACE_POS);
+    }
+
+    private void toBottom() {
+        this.setPosition(TrapConstants.RESET_POS);
+    }
+
+    public Command toBottomCommand() {
+        return runOnce(this::toBottom);
+    }
+
+    public Command toTopCommand() {
+        return runOnce(this::toTop);
+    }
+
     public Command stop() {
         return runOnce(() -> elevator.stopMotor());
     }

diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java
@@ -164,7 +164,7 @@ public static final class TrapConstants {
         public static final int CLAW_CAN_ID = 16;
         public static final double ELEVATOR_DEADBAND = .3;
         public static final double OUTTAKE_TIME = .2;
-        public static final double CLAW_POSITION_MULTIPLIER = 2;
+        public static final double CLAW_POSITION_MULTIPLIER = 1.83;
 
 
         public static final int CLAW_CURRENT_LIMIT = 7;
@@ -176,17 +176,15 @@ public static final class TrapConstants {
 
         public static final int ELEVATOR_MOTOR_CURRENT_LIMIT = 20; // amps
 
-        public static final double TRAP_P = 0.01;
-        public static final double TRAP_I = 0;
-        public static final double TRAP_D = 0;
+        public static final PIDConstants TRAP_PID = new PIDConstants(0.5, 0, 0);
 
         // TODO: set these values
-        public static final double TRAP_POS = 0;
         public static final double RESET_POS = 0;
         public static final double INTAKE_TIME = 0;
-        public static final double CLAW_OUTTAKE = 0;
-        public static final double CLAW_INTAKE = 0;
-        public static final double TRAP_PLACE_POS = 0;
+        public static final double CLAW_OUTTAKE = -1;
+        public static final double CLAW_INTAKE = 1;
+        public static final double TRAP_PLACE_POS = 0.48;
+        public static final double AMP_PLACE_POS = 0.48;
 
         public static final double CLAW_HAS_PIECE_UPPER_LIMIT = 0;
         public static final double CLAW_HAS_PIECE_LOWER_LIMIT = -0.25;